From b31e125e380043b1ebf98f41aa55675f0a56a84a Mon Sep 17 00:00:00 2001 From: phinc23 <60949293+phinc23@users.noreply.github.com> Date: Sun, 19 May 2024 00:53:18 -0400 Subject: [PATCH] =?UTF-8?q?=F0=9F=94=96=20Release/4.1.0=20(#668)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Start of work on motors docs * Add descriptions for color files. * Error.h * gps.hpp done * Gps.h done * More motors docs * Motor docs and fixed the build * added changes to device.hpp, distance.hpp and imu.hpp * Revert "added changes to device.hpp, distance.hpp and" This reverts commit 380a2dee315ee31c468b794d9b98a49e8c85605d. * Do the link files * attempt to fix issue with ADI * Fix ADI Doc value * Remove accidental addition of extra bracket * AbstractMotor work * attempt to refix something * yep looks like that was the culprit * Add it back because it was breaking stuff * The serial files * Optical cpp examples * Get rid of legacy api file (nobody uses PROS 2 API anymore) * Update documentation for the llemu api * Update rtos.h * Attempt to fix adi doc bracketing * Update rtos.h * Fix up formatting on rtos.h * try something really stupid * Finished functions in motors.h * progress on motors.hpp * Update adi bracketting * Fix adi.h bracketing again * Comment brackets in rtos.h * Try removing \enum and \typedef to see if that breaks m.css * Try removing \def * Try some more things to fix CI * Strip name and group commands to see if that lets the CI get further * More more motors.hpp work * Update rtos.h * More rtos.h updates * abstract motor work * rotation.h and rotation.hpp documentation * Fixes to code examples. * do some of the _all functions * more stuff * imu_set_euler * MOTORS.HPP DONE * added example for device.h * fix typo * added example for device.hpp * fix issue * fix formatting * fix and add example in distance.h * WIP on rtos.hpp * ext_adi.h file * fix typo in distance.h * motors and motor group fixes * Trying to fix motor linking * Wip in rtos * update example in device.hpp * updated device.hpp examples * updated distance.hpp examples * fix typo in device.h * fix typo in device.hpp * Vision c++ header docs * Update motors * docs for mics, screen and abstract_motor(enum) * Fix up device file, apix.h work * added example for constructors in device.hpp and distance.hpp * imu.hpp function examples * Small edit for imu.hpp function examples * Added header for imu.hpp constructor * Small edit for imu.hpp constructor header * Remove links * Fix up color and device files * Finish rtos.h and rtos.hpp * Fix cpp-link * Fixes to motors docs * Rotation.hpp docs updates * Reverse Flag param in Rotation constructor * Formatting fixes * Fix motor groups and fix colors * fixed file for motor group * Fix bracketing * Fix MG docs * Remove PROS 2 references, fix up readmes * Another fix * Most of the work done for motor groups * Fix missing end codes * Almost done * FINISHED MOTOR GROUPS * added namespace for screen.hpp * Update version * Fix a couple enum/struct issues * :memo: Some fixes for motor headers * :memo: Some fixes for enums * 🐛Motor bug fixes and add new set_gearing (#611) * ✨Add list_files function (#612) * Add list_files * renamed c function * Clarify the docs * :sparkles: ADI Get Port (#613) * Initial Commit. Added get_port() for adi. * Second commit. Fixed changes. * 🐛Vex link default override (#614) * Initial Commit. Changed defualt param. * Fixed param docs * ✨Field Control State Getter (#608) * Field Control State Getter * Fixed merge issues * Removed left over testing code * change macros to enum/functions --------- Co-authored-by: noam987 Co-authored-by: noam987 <50681033+noam987@users.noreply.github.com> * Added vexFileSync when writing to files (#619) * 🐛Fix rotation sensor reversed port (#618) * 🐛 Field Control Getter Return Value (#627) * Field Control State Getter * Fixed merge issues * Removed left over testing code * change macros to enum/functions * fix return value issue * fixed version --------- Co-authored-by: noam987 Co-authored-by: noam987 <50681033+noam987@users.noreply.github.com> * ✨default the VFS to the sd card (#621) * Fix motor voltage limit port mutex returning * Initial implementation for get_all_devices * Remove use of templating * modify gps functions * add example code to gps header files * revert api.h and version * add newline at end of gps.c * add newline at end of version * Completed get_all_devices member function for all critical devices * Fix compile issues * 🐛ADI mutex fix (#633) * Fixed duplicate zero indexing in ext_adi_led_set_pixel as well as validate_type * testing * Fixed zero indexing * fix merge * removes an extra -1 * Add missing -1 * Last -1 * Fix return without releasing mutex --------- Co-authored-by: noam987 * ✨Adds static getters for some devices (#653) * imu_v1 * revert main.cpp * Add injector for gps * rename to _ casing and add vision sensor * 📝 PROS 4: Documentation Fixes for optical and distance sensor (#654) * Documentation Fixes for optical sensor * Distance sensor documentation fixes * Added alias function get_distance for get for distance sensor * Update get_distance func header for distance sensor * 🐛Fixed Imu::is_calibrating function for PROS 4 #626 (#629) * Fixed Imu::is_calibrating function Updated imu status enums to properly reflect values returned by get_status. Also modifed imu::is_calibrating so it returns the correct value. * Bugfixes from the pros 3 version --------- Co-authored-by: noam987 Co-authored-by: noam987 <50681033+noam987@users.noreply.github.com> * 🐛Remove = overload from motor groups (#656) * 📝Update docs for pre-release (#657) * Update version * :bug: Fix pros::E_TEXT_LARGE_CENTER Simple Name * Update version numbers * 🐛Motor bug fixes and add new set_gearing (#611) * ✨Add list_files function (#612) * Add list_files * renamed c function * Clarify the docs * :sparkles: ADI Get Port (#613) * Initial Commit. Added get_port() for adi. * Second commit. Fixed changes. * 🐛Vex link default override (#614) * Initial Commit. Changed defualt param. * Fixed param docs * ✨Field Control State Getter (#608) * Field Control State Getter * Fixed merge issues * Removed left over testing code * change macros to enum/functions --------- Co-authored-by: noam987 Co-authored-by: noam987 <50681033+noam987@users.noreply.github.com> * Added vexFileSync when writing to files (#619) * 🐛Fix rotation sensor reversed port (#618) * 🐛 Field Control Getter Return Value (#627) * Field Control State Getter * Fixed merge issues * Removed left over testing code * change macros to enum/functions * fix return value issue * fixed version --------- Co-authored-by: noam987 Co-authored-by: noam987 <50681033+noam987@users.noreply.github.com> * ✨default the VFS to the sd card (#621) * Fix motor voltage limit port mutex returning * Initial implementation for get_all_devices * Remove use of templating * modify gps functions * add example code to gps header files * revert api.h and version * add newline at end of gps.c * add newline at end of version * Completed get_all_devices member function for all critical devices * Fix compile issues * 🐛ADI mutex fix (#633) * Fixed duplicate zero indexing in ext_adi_led_set_pixel as well as validate_type * testing * Fixed zero indexing * fix merge * removes an extra -1 * Add missing -1 * Last -1 * Fix return without releasing mutex --------- Co-authored-by: noam987 * ✨Adds static getters for some devices (#653) * imu_v1 * revert main.cpp * Add injector for gps * rename to _ casing and add vision sensor * 📝 PROS 4: Documentation Fixes for optical and distance sensor (#654) * Documentation Fixes for optical sensor * Distance sensor documentation fixes * Added alias function get_distance for get for distance sensor * Update get_distance func header for distance sensor * 🐛Fixed Imu::is_calibrating function for PROS 4 #626 (#629) * Fixed Imu::is_calibrating function Updated imu status enums to properly reflect values returned by get_status. Also modifed imu::is_calibrating so it returns the correct value. * Bugfixes from the pros 3 version --------- Co-authored-by: noam987 Co-authored-by: noam987 <50681033+noam987@users.noreply.github.com> * 🐛Remove = overload from motor groups (#656) --------- Co-authored-by: Richard Stump Co-authored-by: Will Xu Co-authored-by: Will Xu <54247087+WillXuCodes@users.noreply.github.com> Co-authored-by: phinc23 <60949293+phinc23@users.noreply.github.com> Co-authored-by: Yuechan Li <55300543+CChheerryyll@users.noreply.github.com> Co-authored-by: Sprocket Riggs <34726295+aberiggs@users.noreply.github.com> Co-authored-by: Cooper7196 Co-authored-by: Gavin-Niederman Co-authored-by: aberiggs Co-authored-by: Yuechan Li Co-authored-by: Gracelu128 <112266075+Gracelu128@users.noreply.github.com> Co-authored-by: Richard Li <61027374+R11G@users.noreply.github.com> * Update version numbers (#658) * 🐛Fix imu get device typo (#659) * Fix motor docs * minor docs fixes * 📝 Fix typo in main.h (#662) * Add documentation to literals * 📝 Fix typo in main.cpp (#663) * Fix a typo in main.h * Fix a typo in main.cpp * 🐛Converts appending, and constructors to take in AbstractMotor& instead of MotorGroup& (#661) * 📝Update docs for pre-release (#657) * Update version * :bug: Fix pros::E_TEXT_LARGE_CENTER Simple Name * Update version numbers * 🐛Motor bug fixes and add new set_gearing (#611) * ✨Add list_files function (#612) * Add list_files * renamed c function * Clarify the docs * :sparkles: ADI Get Port (#613) * Initial Commit. Added get_port() for adi. * Second commit. Fixed changes. * 🐛Vex link default override (#614) * Initial Commit. Changed defualt param. * Fixed param docs * ✨Field Control State Getter (#608) * Field Control State Getter * Fixed merge issues * Removed left over testing code * change macros to enum/functions --------- Co-authored-by: noam987 Co-authored-by: noam987 <50681033+noam987@users.noreply.github.com> * Added vexFileSync when writing to files (#619) * 🐛Fix rotation sensor reversed port (#618) * 🐛 Field Control Getter Return Value (#627) * Field Control State Getter * Fixed merge issues * Removed left over testing code * change macros to enum/functions * fix return value issue * fixed version --------- Co-authored-by: noam987 Co-authored-by: noam987 <50681033+noam987@users.noreply.github.com> * ✨default the VFS to the sd card (#621) * Fix motor voltage limit port mutex returning * Initial implementation for get_all_devices * Remove use of templating * modify gps functions * add example code to gps header files * revert api.h and version * add newline at end of gps.c * add newline at end of version * Completed get_all_devices member function for all critical devices * Fix compile issues * 🐛ADI mutex fix (#633) * Fixed duplicate zero indexing in ext_adi_led_set_pixel as well as validate_type * testing * Fixed zero indexing * fix merge * removes an extra -1 * Add missing -1 * Last -1 * Fix return without releasing mutex --------- Co-authored-by: noam987 * ✨Adds static getters for some devices (#653) * imu_v1 * revert main.cpp * Add injector for gps * rename to _ casing and add vision sensor * 📝 PROS 4: Documentation Fixes for optical and distance sensor (#654) * Documentation Fixes for optical sensor * Distance sensor documentation fixes * Added alias function get_distance for get for distance sensor * Update get_distance func header for distance sensor * 🐛Fixed Imu::is_calibrating function for PROS 4 #626 (#629) * Fixed Imu::is_calibrating function Updated imu status enums to properly reflect values returned by get_status. Also modifed imu::is_calibrating so it returns the correct value. * Bugfixes from the pros 3 version --------- Co-authored-by: noam987 Co-authored-by: noam987 <50681033+noam987@users.noreply.github.com> * 🐛Remove = overload from motor groups (#656) --------- Co-authored-by: Richard Stump Co-authored-by: Will Xu Co-authored-by: Will Xu <54247087+WillXuCodes@users.noreply.github.com> Co-authored-by: phinc23 <60949293+phinc23@users.noreply.github.com> Co-authored-by: Yuechan Li <55300543+CChheerryyll@users.noreply.github.com> Co-authored-by: Sprocket Riggs <34726295+aberiggs@users.noreply.github.com> Co-authored-by: Cooper7196 Co-authored-by: Gavin-Niederman Co-authored-by: aberiggs Co-authored-by: Yuechan Li Co-authored-by: Gracelu128 <112266075+Gracelu128@users.noreply.github.com> Co-authored-by: Richard Li <61027374+R11G@users.noreply.github.com> * Fix motor docs * minor docs fixes * Compiles, haven't tested past that * Revert main.cpp * remove extra includes in main * another small style fix * ixed issue found during testing --------- Co-authored-by: Richard Stump Co-authored-by: Will Xu Co-authored-by: Will Xu <54247087+WillXuCodes@users.noreply.github.com> Co-authored-by: phinc23 <60949293+phinc23@users.noreply.github.com> Co-authored-by: Yuechan Li <55300543+CChheerryyll@users.noreply.github.com> Co-authored-by: Sprocket Riggs <34726295+aberiggs@users.noreply.github.com> Co-authored-by: Cooper7196 Co-authored-by: Gavin-Niederman Co-authored-by: aberiggs Co-authored-by: Yuechan Li Co-authored-by: Gracelu128 <112266075+Gracelu128@users.noreply.github.com> Co-authored-by: Richard Li <61027374+R11G@users.noreply.github.com> * 📝Docs + implementation forgotten for rotation sensor (#665) * Fix compile issues * Update version nums --------- Co-authored-by: Noam987 Co-authored-by: Will Xu Co-authored-by: Richard Stump Co-authored-by: Aaryan Gautam <64756497+aaryan-gautam@users.noreply.github.com> Co-authored-by: Andrew Lu Co-authored-by: aberiggs Co-authored-by: Grace Lu Co-authored-by: AnnZi Co-authored-by: Jerrylum Co-authored-by: Will Xu <54247087+WillXuCodes@users.noreply.github.com> Co-authored-by: noam987 <50681033+noam987@users.noreply.github.com> Co-authored-by: Yuechan Li <55300543+CChheerryyll@users.noreply.github.com> Co-authored-by: Sprocket Riggs <34726295+aberiggs@users.noreply.github.com> Co-authored-by: Cooper7196 Co-authored-by: Gavin-Niederman Co-authored-by: Yuechan Li Co-authored-by: Gracelu128 <112266075+Gracelu128@users.noreply.github.com> Co-authored-by: Richard Li <61027374+R11G@users.noreply.github.com> Co-authored-by: THERocky <101498190+Rocky14683@users.noreply.github.com> --- include/api.h | 6 +- include/main.h | 2 +- include/pros/abstract_motor.hpp | 49 +- include/pros/adi.hpp | 51 +++ include/pros/device.hpp | 45 +- include/pros/distance.hpp | 100 +++- include/pros/gps.h | 530 ++++++++++++++------- include/pros/gps.hpp | 492 +++++++++++++++----- include/pros/imu.h | 29 +- include/pros/imu.hpp | 349 ++++++++------ include/pros/link.h | 4 +- include/pros/link.hpp | 4 +- include/pros/misc.h | 230 ++++++---- include/pros/misc.hpp | 133 ++++-- include/pros/motor_group.hpp | 539 ++++++++++++---------- include/pros/motors.h | 2 +- include/pros/motors.hpp | 785 ++++++++++++++++---------------- include/pros/optical.hpp | 100 ++-- include/pros/rotation.hpp | 162 ++++--- include/pros/screen.h | 15 +- include/pros/serial.hpp | 30 +- include/pros/vision.hpp | 416 +++++++++-------- src/devices/controller.c | 20 + src/devices/controller.cpp | 10 + src/devices/vdml_adi.cpp | 39 +- src/devices/vdml_device.cpp | 30 +- src/devices/vdml_distance.cpp | 13 + src/devices/vdml_ext_adi.c | 120 +++-- src/devices/vdml_gps.c | 120 ++++- src/devices/vdml_gps.cpp | 87 +++- src/devices/vdml_imu.c | 89 ++-- src/devices/vdml_imu.cpp | 39 +- src/devices/vdml_motorgroup.cpp | 64 ++- src/devices/vdml_motors.c | 2 +- src/devices/vdml_motors.cpp | 26 +- src/devices/vdml_optical.cpp | 9 + src/devices/vdml_rotation.cpp | 27 +- src/devices/vdml_usd.c | 12 + src/devices/vdml_usd.cpp | 4 + src/devices/vdml_vision.cpp | 32 +- src/main.cpp | 19 +- src/system/dev/usd_driver.c | 2 + src/system/dev/vfs.c | 5 +- version | 2 +- 44 files changed, 3108 insertions(+), 1736 deletions(-) diff --git a/include/api.h b/include/api.h index 5bddacfd4..b97e65d61 100644 --- a/include/api.h +++ b/include/api.h @@ -40,10 +40,10 @@ #endif /* __cplusplus */ #define PROS_VERSION_MAJOR 4 -#define PROS_VERSION_MINOR 0 -#define PROS_VERSION_PATCH 6 -#define PROS_VERSION_STRING "4.0.6" +#define PROS_VERSION_MINOR 1 +#define PROS_VERSION_PATCH 0 +#define PROS_VERSION_STRING "4.1.0" #include "pros/adi.h" #include "pros/colors.h" diff --git a/include/main.h b/include/main.h index 9407e1eab..438dc79b7 100644 --- a/include/main.h +++ b/include/main.h @@ -22,7 +22,7 @@ * * For instance, E_CONTROLLER_MASTER has a shorter name: CONTROLLER_MASTER. * E_CONTROLLER_MASTER is pedantically correct within the PROS styleguide, but - * not convienent for most student programmers. + * not convenient for most student programmers. */ #define PROS_USE_SIMPLE_NAMES diff --git a/include/pros/abstract_motor.hpp b/include/pros/abstract_motor.hpp index 1468f35f3..d2a3e31f6 100644 --- a/include/pros/abstract_motor.hpp +++ b/include/pros/abstract_motor.hpp @@ -31,7 +31,7 @@ namespace pros { inline namespace v5 { /** - * \enum motor_brake + * \enum MotorBrake * Indicates the current 'brake mode' of a motor. */ enum class MotorBrake { @@ -42,7 +42,7 @@ enum class MotorBrake { }; /** - * \enum Motor_Encoder_Units + * \enum MotorEncoderUnits * Indicates the units used by the motor encoders. */ enum class MotorEncoderUnits { @@ -53,7 +53,7 @@ enum class MotorEncoderUnits { invalid = INT32_MAX ///< Invalid motor encoder units }; -// Alias for Motor_Encoder_Units +// Alias for MotorEncoderUnits using MotorUnits = MotorEncoderUnits; enum class MotorGears { @@ -70,7 +70,7 @@ enum class MotorGears { }; -// Provide Aliases for Motor_Gears +// Provide Aliases for MotorGears using MotorGearset = MotorGears; using MotorCart = MotorGears; using MotorCartridge = MotorGears; @@ -89,25 +89,6 @@ class AbstractMotor { /// These functions allow programmers to make motors move ///@{ - /** - * Sets the voltage for the motor from -127 to 127. - * - * This is designed to map easily to the input from the controller's analog - * stick for simple opcontrol use. The actual behavior of the motor is - * analogous to use of pros::Motor::move(). - * - * This function uses the following values of errno when an error state is - * reached: - * ENODEV - The port cannot be configured as a motor - * - * \param voltage - * The new motor voltage from -127 to 127 - * - * \return 1 if the operation was successful or PROS_ERR if the operation - * failed, setting errno. - */ - virtual std::int32_t operator=(std::int32_t voltage) const = 0; - /** * Sets the voltage for the motor from -127 to 127. * @@ -778,7 +759,7 @@ class AbstractMotor { * The index of the motor to get the target position of. * By default index is 0, and will return an error for an out of bounds index * - * \return One of Motor_Brake, according to what was set for the + * \return One of MotorBrake, according to what was set for the * motor, or E_MOTOR_BRAKE_INVALID if the operation failed, setting errno. */ virtual MotorBrake get_brake_mode(const std::uint8_t index = 0) const = 0; @@ -794,7 +775,7 @@ class AbstractMotor { * The index of the motor to get the target position of. * By default index is 0, and will return an error for an out of bounds index * - * \return A vector containing Motor_Brake(s), according to what was set for the + * \return A vector containing MotorBrake(s), according to what was set for the * motor(s), or E_MOTOR_BRAKE_INVALID if the operation failed, setting errno. */ virtual std::vector get_brake_mode_all(void) const = 0; @@ -846,7 +827,7 @@ class AbstractMotor { * The index of the motor to get the target position of. * By default index is 0, and will return an error for an out of bounds index * - * \return One of Motor_Units according to what is set for the + * \return One of MotorUnits according to what is set for the * motor or E_MOTOR_ENCODER_INVALID if the operation failed. */ virtual MotorUnits get_encoder_units(const std::uint8_t index = 0) const = 0; @@ -862,7 +843,7 @@ class AbstractMotor { * The index of the motor to get the target position of. * By default index is 0, and will return an error for an out of bounds index * - * \return A vector of Motor_Units according to what is set for the + * \return A vector of MotorUnits according to what is set for the * motor(s) or E_MOTOR_ENCODER_INVALID if the operation failed. */ virtual std::vector get_encoder_units_all(void) const = 0; @@ -878,8 +859,8 @@ class AbstractMotor { * The index of the motor to get the target position of. * By default index is 0, and will return an error for an out of bounds index * - * \return One of Motor_Gears according to what is set for the motor, - * or pros::Motor_Gears::invalid if the operation failed. + * \return One of MotorGears according to what is set for the motor, + * or pros::MotorGears::invalid if the operation failed. */ virtual MotorGears get_gearing(const std::uint8_t index = 0) const = 0; @@ -894,8 +875,8 @@ class AbstractMotor { * The index of the motor to get the target position of. * By default index is 0, and will return an error for an out of bounds index * - * \return A vector of Motor_Gears according to what is set for the motor(s), - * or pros::Motor_Gears::invalid if the operation failed. + * \return A vector of MotorGears according to what is set for the motor(s), + * or pros::MotorGears::invalid if the operation failed. */ virtual std::vector get_gearing_all(void) const = 0; @@ -977,7 +958,7 @@ class AbstractMotor { virtual std::vector is_reversed_all(void) const = 0; /** - * Sets one of Motor_Brake to the motor. Works with the C enum + * Sets one of MotorBrake to the motor. Works with the C enum * and the C++ enum class. * * This function uses the following values of errno when an error state is @@ -985,7 +966,7 @@ class AbstractMotor { * ENODEV - The port cannot be configured as a motor * * \param mode - * The Motor_Brake to set for the motor + * The MotorBrake to set for the motor * * \param index Optional parameter. * The index of the motor to get the target position of. @@ -1018,7 +999,7 @@ class AbstractMotor { virtual std::int32_t set_current_limit(const std::int32_t limit, const std::uint8_t index = 0) const = 0; virtual std::int32_t set_current_limit_all(const std::int32_t limit) const = 0; /** - * Sets one of Motor_Units for the motor encoder. Works with the C + * Sets one of MotorUnits for the motor encoder. Works with the C * enum and the C++ enum class. * * This function uses the following values of errno when an error state is diff --git a/include/pros/adi.hpp b/include/pros/adi.hpp index a12bd4ac1..75429c981 100644 --- a/include/pros/adi.hpp +++ b/include/pros/adi.hpp @@ -190,6 +190,33 @@ class Port { */ std::int32_t set_value(std::int32_t value) const; + /** + * Gets the port of the sensor. + * + * \return returns a tuple of integer ports. + * + * \note The parts of the tuple are {smart port, adi port, second adi port (when applicable)}. + * + * + * \b Example + * \code + * #define DIGITAL_SENSOR_PORT 1 // 'A' + * + * void initialize() { + * pros::adi::AnalogIn sensor (DIGITAL_SENSOR_PORT); + * + * // Getting values from the tuple using std::get + * int sensorSmartPort = std::get<0>(sensor.get_port()); // First value + * int sensorAdiPort = std::get<1>(sensor.get_port()); // Second value + * + * // Prints the first and second value from the port tuple (The Adi Port. The first value is the Smart Port) + * printf("Sensor Smart Port: %d\n", sensorSmartPort); + * printf("Sensor Adi Port: %d\n", sensorAdiPort); + * } + * \endcode + */ + virtual ext_adi_port_tuple_t get_port() const; + protected: std::uint8_t _smart_port; std::uint8_t _adi_port; @@ -396,6 +423,8 @@ class AnalogIn : protected Port { * value calibrated HR: (16 bit calibrated value), value: (12 bit value)] */ friend std::ostream& operator<<(std::ostream& os, pros::adi::AnalogIn& analog_in); + + using Port::get_port; }; ///@} @@ -484,6 +513,8 @@ class AnalogOut : private Port { * \endcode */ using Port::set_value; + + using Port::get_port; /** * This is the overload for the << operator for printing to streams @@ -595,6 +626,8 @@ class DigitalOut : private Port { */ using Port::set_value; + using Port::get_port; + /** * This is the overload for the << operator for printing to streams * @@ -731,6 +764,8 @@ class DigitalIn : private Port { * value: (value)] */ friend std::ostream& operator<<(std::ostream& os, pros::adi::DigitalIn& digital_in); + + using Port::get_port; }; ///@} @@ -875,6 +910,8 @@ class Motor : private Port { * \endcode */ using Port::get_value; + + using Port::get_port; }; ///@} @@ -1004,6 +1041,11 @@ class Encoder : private Port { * value: (value)] */ friend std::ostream& operator<<(std::ostream& os, pros::adi::Encoder& encoder); + ext_adi_port_tuple_t get_port() const override; + + private: + ext_adi_port_pair_t _port_pair; + }; ///@} @@ -1108,6 +1150,8 @@ class Ultrasonic : private Port { * \endcode */ std::int32_t get_value() const; + + using Port::get_port; }; ///@} @@ -1259,6 +1303,8 @@ class Gyro : private Port { * \endcode */ std::int32_t reset() const; + + using Port::get_port; }; ///@} @@ -1419,6 +1465,9 @@ class Potentiometer : public AnalogIn { * Prints in format(this below is all in one line with no new line): */ friend std::ostream& operator<<(std::ostream& os, pros::adi::Potentiometer& potentiometer); + + using Port::get_port; + }; ///@} @@ -1710,6 +1759,8 @@ class Led : protected Port { */ std::int32_t length(); + using Port::get_port; + protected: std::vector _buffer; }; diff --git a/include/pros/device.hpp b/include/pros/device.hpp index 7fcee80ce..5055d4894 100644 --- a/include/pros/device.hpp +++ b/include/pros/device.hpp @@ -90,7 +90,7 @@ class Device { * } * \endcode */ - std::uint8_t get_port(void); + std::uint8_t get_port(void) const; /** * Checks if the device is installed. @@ -138,6 +138,49 @@ class Device { pros::DeviceType get_plugged_type() const; + /** + * Gets the type of device on a given port. + * + * This function uses the following values of errno when an error state is + * reached: + * EACCES - Mutex of port cannot be taken (access denied). + * + * \param port The V5 port number from 1-21 + * + * \return The device type as an enum. + * + * \b Example + * \code + * #define DEVICE_PORT 1 + * + * void opcontrol() { + * while (true) { + * DeviceType dt = pros::Device::get_plugged_type(DEVICE_PORT); + * printf("device plugged type: {plugged type: %d}\n", dt); + * delay(20); + * } + * } + * \endcode + */ + static pros::DeviceType get_plugged_type(std::uint8_t port); + + /** + * Gets all devices of a given device type. + * + * \param device_type The pros::DeviceType enum that matches the type of device desired. + * + * \return A vector of Device objects for the given device type. + * + * \b Example + * \code + * void opcontrol() { + * std::vector motor_devices = pros::Device::get_all_devices(pros::DeviceType::motor); // All Device objects are motors + * } + * \endcode + */ + + static std::vector get_all_devices(pros::DeviceType device_type = pros::DeviceType::undefined); + protected: /** * Creates a Device object. diff --git a/include/pros/distance.hpp b/include/pros/distance.hpp index 288dff10d..ae93928b5 100644 --- a/include/pros/distance.hpp +++ b/include/pros/distance.hpp @@ -12,7 +12,7 @@ * This Source Code Form is subject to the terms of the Mozilla Public * License, v. 2.0. If a copy of the MPL was not distributed with this * file, You can obtain one at http://mozilla.org/MPL/2.0/. - * + * * \defgroup cpp-distance VEX Distance Sensor C++ API */ @@ -34,7 +34,7 @@ class Distance : public Device { /** * \addtogroup cpp-distance * @{ - */ + */ public: /** * Creates a Distance Sensor object for the given port. @@ -56,8 +56,9 @@ class Distance : public Device { * } * \endcode */ - explicit Distance(const std::uint8_t port); + Distance(const std::uint8_t port); + Distance(const Device& device) : Distance(device.get_port()){}; /** * Get the currently measured distance from the sensor in mm * @@ -67,14 +68,14 @@ class Distance : public Device { * ENODEV - The port cannot be configured as an Distance Sensor * * \return The distance value or PROS_ERR if the operation failed, setting - * errno. + * errno. Will return 9999 if the sensor can not detect an object. * * \b Example * \code * #define DISTANCE_PORT 1 - * + * * void opcontrol() { - Distance distance(DISTANCE_PORT); + Distance distance(DISTANCE_PORT); * while (true) { * printf("Distance confidence: %d\n", distance.get()); * delay(20); @@ -84,6 +85,48 @@ class Distance : public Device { */ virtual std::int32_t get(); + /** + * Get the currently measured distance from the sensor in mm. + * \note This function is identical to get(). + * + * This function uses the following values of errno when an error state is + * reached: + * ENXIO - The given value is not within the range of V5 ports (1-21). + * ENODEV - The port cannot be configured as an Distance Sensor + * + * \return The distance value or PROS_ERR if the operation failed, setting + * errno. Will return 9999 if the sensor can not detect an object. + * + * \b Example + * \code + * #define DISTANCE_PORT 1 + * + * void opcontrol() { + Distance distance(DISTANCE_PORT); + * while (true) { + * printf("Distance confidence: %d\n", distance.get_distance()); + * delay(20); + * } + * } + * \endcode + */ + virtual std::int32_t get_distance(); + + /** + * Gets all distance sensors. + * + * \return A vector of Distance sensor objects. + * + * \b Example + * \code + * void opcontrol() { + * std::vector distance_all = pros::Distance::get_all_devices(); // All distance sensors that are + * connected + * } + * \endcode + */ + static std::vector get_all_devices(); + /** * Get the confidence in the distance reading * @@ -102,9 +145,9 @@ class Distance : public Device { * \b Example * \code * #define DISTANCE_PORT 1 - * + * * void opcontrol() { - Distance distance(DISTANCE_PORT); + Distance distance(DISTANCE_PORT); * while (true) { * printf("Distance confidence: %d\n", distance.get_confidence()); * delay(20); @@ -127,14 +170,14 @@ class Distance : public Device { * ENODEV - The port cannot be configured as an Distance Sensor * * \return The size value or PROS_ERR if the operation failed, setting - * errno. + * errno. Will return -1 if the sensor is not able to determine object size. * * \b Example * \code * #define DISTANCE_PORT 1 - * + * * void opcontrol() { - Distance distance(DISTANCE_PORT); + Distance distance(DISTANCE_PORT); * while (true) { * printf("Distance confidence: %d\n", distance.get_object_size()); * delay(20); @@ -154,10 +197,10 @@ class Distance : public Device { * * \return The velocity value or PROS_ERR if the operation failed, setting * errno. - * + * * \b Example * \code - * + * * void opcontrol() { * Distance distance(DISTANCE_PORT); * while (true) { @@ -169,23 +212,36 @@ class Distance : public Device { */ virtual double get_object_velocity(); - /** - * This is the overload for the << operator for printing to streams - * - * Prints in format(this below is all in one line with no new line): - * Distance [port: (port number), distance: (distance), confidence: (confidence), - * object size: (object size), object velocity: (object velocity)] - */ + /** + * This is the overload for the << operator for printing to streams + * + * Prints in format(this below is all in one line with no new line): + * Distance [port: (port number), distance: (distance), confidence: (confidence), + * object size: (object size), object velocity: (object velocity)] + */ friend std::ostream& operator<<(std::ostream& os, pros::Distance& distance); - + private: ///@} }; namespace literals { +/** + * Constructs a Distance sensor object from a literal ending in _dist via calling the constructor + * + * \return a pros::Distance for the corresponding port + * + * \b Example + * \code + * using namespace pros::literals; + * void opcontrol() { + * pros::Distance dist = 2_dist; //Makes an dist object on port 2 + * } + * \endcode + */ const pros::Distance operator"" _dist(const unsigned long long int d); } // namespace literals -} +} // namespace v5 } // namespace pros #endif diff --git a/include/pros/gps.h b/include/pros/gps.h index 9304e0f95..c4d4fdfe6 100644 --- a/include/pros/gps.h +++ b/include/pros/gps.h @@ -26,7 +26,7 @@ #ifdef __cplusplus extern "C" { namespace pros { -#endif +#endif /** * \ingroup c-gps @@ -55,23 +55,36 @@ typedef struct __attribute__((__packed__)) gps_status_s { double x; /// Y Position (meters) double y; - /// Percieved Pitch based on GPS + IMU + /// Perceived Pitch based on GPS + IMU double pitch; - /// Percieved Roll based on GPS + IMU + /// Perceived Roll based on GPS + IMU double roll; - /// Percieved Yaw based on GPS + IMU + /// Perceived Yaw based on GPS + IMU double yaw; } gps_status_s_t; +/** + * \struct gps_orientation_s_t + */ +typedef struct __attribute__((__packed__)) gps_orientation_s { + /// Perceived Pitch based on GPS + IMU + double pitch; + /// Perceived Roll based on GPS + IMU + double roll; + /// Perceived Yaw based on GPS + IMU + double yaw; +} gps_orientation_s_t; + + /** * \struct gps_raw_s */ struct gps_raw_s { - /// Percieved Pitch based on GPS + IMU + /// Perceived Pitch based on GPS + IMU double x; - /// Percieved Roll based on GPS + IMU + /// Perceived Roll based on GPS + IMU double y; - /// Percieved Yaw based on GPS + IMU + /// Perceived Yaw based on GPS + IMU double z; }; @@ -166,7 +179,7 @@ int32_t gps_initialize_full(uint8_t port, double xInitial, double yInitial, doub int32_t gps_set_offset(uint8_t port, double xOffset, double yOffset); /** - * Gets the position and roll, yaw, and pitch of the GPS. + * Get the GPS's cartesian location relative to the center of turning/origin in meters. * * This function uses the following values of errno when an error state is * reached: @@ -176,30 +189,93 @@ int32_t gps_set_offset(uint8_t port, double xOffset, double yOffset); * * \param port * The V5 GPS port number from 1-21 + * \return A struct (gps_position_s_t) containing the X and Y values if the operation + * failed, setting errno. + * + * \b Example + * \code + * #define GPS_PORT 1 + * + * void opcontrol() { + * gps_position_s_t pos; + * + * while (true) { + * pos = gps_get_offset(GPS_PORT); + * screen_print(TEXT_MEDIUM, 1, "X Offset: %4d, Y Offset: %4d", pos.x, pos.y); + * delay(20); + * } + * } + * \endcode + */ +gps_position_s_t gps_get_offset(uint8_t port); + +/** + * Sets the robot's location relative to the center of the field in meters. * - * \return A struct (gps_status_s_t) containing values mentioned above. - * If the operation failed, all the structure's members are filled with - * PROS_ERR_F and errno is set. + * This function uses the following values of errno when an error state is + * reached: + * ENXIO - The given value is not within the range of V5 ports (1-21). + * ENODEV - The port cannot be configured as a GPS + * EAGAIN - The sensor is still calibrating * + * \param port + * The V5 GPS port number from 1-21 + * \param xInitial + * Initial 4-Quadrant X Position, with (0,0) being at the center of the field (meters) + * \param yInitial + * Initial 4-Quadrant Y Position, with (0,0) being at the center of the field (meters) + * \param headingInitial + * Heading with 0 being north on the field, in degrees [0,360) going clockwise + * \return 1 if the operation was successful or PROS_ERR if the operation + * failed, setting errno. + * * \b Example * \code * #define GPS_PORT 1 + * #define X_INITIAL -1.15 + * #define Y_INITIAL 1.45 + * #define HEADING_INITIAL 90 + * + * void initialize() { + * gps_set_position(GPS_PORT, X_INITIAL, Y_INITIAL, HEADING_INITIAL); + * } + * \endcode + */ +int32_t gps_set_position(uint8_t port, double xInitial, double yInitial, double headingInitial); + +/** + * Set the GPS sensor's data rate in milliseconds, only applies to IMU on GPS. * - * void opcontrol() { - * gps_status_s_t status; + * This function uses the following values of errno when an error state is + * reached: + * ENXIO - The given value is not within the range of V5 ports (1-21). + * ENODEV - The port cannot be configured as a GPS + * EAGAIN - The sensor is still calibrating * + * \param port + * The V5 GPS port number from 1-21 + * \param rate + * Data rate in milliseconds (Minimum: 5 ms) + * \return 1 if the operation was successful or PROS_ERR if the operation + * failed, setting errno. + * + * \b Example + * \code + * #define GPS_PORT 1 + * #define GPS_DATA_RATE 5 + * + * void initialize() { + * gps_set_data_rate(GPS_PORT, GPS_DATA_RATE); * while (true) { - * status = gps_get_status(GPS_PORT); - * printf("X: %f, Y: %f, Pitch: %f, Roll: %f, Yaw: %f\n", status.x, status.y, status.pitch, status.roll, status.yaw); - * delay(20); + * // Do something * } * } * \endcode */ -gps_status_s_t gps_get_status(uint8_t port); +int32_t gps_set_data_rate(uint8_t port, uint32_t rate); /** - * Gets the x and y position on the field of the GPS in meters. + * Get the possible RMS (Root Mean Squared) error in meters for GPS position. * * This function uses the following values of errno when an error state is * reached: @@ -210,29 +286,57 @@ gps_status_s_t gps_get_status(uint8_t port); * \param port * The V5 GPS port number from 1-21 * - * \return A struct (gps_position_s_t) containing values mentioned above. + * \return Possible RMS (Root Mean Squared) error in meters for GPS position. + * If the operation failed, returns PROS_ERR_F and errno is set. + * + * \b Example + * \code + * #define GPS_PORT 1 + * + * void opcontrol() { + * double error; + * error = gps_get_error(GPS_PORT); + * screen_print(TEXT_MEDIUM, 1, "Error: %4d", error); + * } + * \endcode + */ +double gps_get_error(uint8_t port); + +/** + * Gets the position and roll, yaw, and pitch of the GPS. + * + * This function uses the following values of errno when an error state is + * reached: + * ENXIO - The given value is not within the range of V5 ports (1-21). + * ENODEV - The port cannot be configured as a GPS + * EAGAIN - The sensor is still calibrating + * + * \param port + * The V5 GPS port number from 1-21 + * + * \return A struct (gps_status_s_t) containing values mentioned above. * If the operation failed, all the structure's members are filled with * PROS_ERR_F and errno is set. - * + * * \b Example * \code * #define GPS_PORT 1 * * void opcontrol() { - * gps_position_s_t position; + * gps_status_s_t status; * * while (true) { - * position = gps_get_position(GPS_PORT); - * printf("X: %f, Y: %f\n", position.x, position.y); + * status = gps_get_position_and_orientation(GPS_PORT); + * printf("X: %f, Y: %f, Pitch: %f, Roll: %f, Yaw: %f\n", status.x, status.y, status.pitch, status.roll, status.yaw); * delay(20); * } * } * \endcode */ -gps_position_s_t gps_get_position(uint8_t port); +gps_status_s_t gps_get_position_and_orientation(uint8_t port); /** - * Get the GPS's raw gyroscope values + * Gets the x and y position on the field of the GPS in meters. * * This function uses the following values of errno when an error state is * reached: @@ -242,60 +346,63 @@ gps_position_s_t gps_get_position(uint8_t port); * * \param port * The V5 GPS port number from 1-21 - * \return The raw gyroscope values. If the operation failed, all the - * structure's members are filled with PROS_ERR_F and errno is set. * + * \return A struct (gps_position_s_t) containing values mentioned above. + * If the operation failed, all the structure's members are filled with + * PROS_ERR_F and errno is set. + * * \b Example * \code * #define GPS_PORT 1 * * void opcontrol() { - * gps_gyro_s_t gyro; + * gps_position_s_t position; * * while (true) { - * gyro = gps_get_gyro(GPS_PORT); - * printf("Gyro: %f %f %f\n", gyro.x, gyro.y, gyro.z); + * position = gps_get_position(GPS_PORT); + * printf("X: %f, Y: %f\n", position.x, position.y); * delay(20); * } * } * \endcode */ -gps_gyro_s_t gps_get_gyro_rate(uint8_t port); +gps_position_s_t gps_get_position(uint8_t port); /** - * Get the GPS's raw accelerometer values + * Gets the X position in meters of the robot relative to the starting position. * * This function uses the following values of errno when an error state is * reached: * ENXIO - The given value is not within the range of V5 ports (1-21). - * ENODEV - The port cannot be configured as an GPS + * ENODEV - The port cannot be configured as a GPS * EAGAIN - The sensor is still calibrating * * \param port - * The V5 GPS's port number from 1-21 - * \return The raw accelerometer values. If the operation failed, all the - * structure's members are filled with PROS_ERR_F and errno is set. + * The V5 GPS port number from 1-21 + * + * \return The X position in meters. If the operation failed, + * returns PROS_ERR_F and errno is set. * * \b Example * \code * #define GPS_PORT 1 * * void opcontrol() { - * gps_accel_s_t accel; + * double pos_x; * * while (true) { - * accel = gps_get_accel(GPS_PORT); - * printf("X: %f, Y: %f, Z: %f\n", accel.x, accel.y, accel.z); + * pos_x = gps_get_position_x(GPS_PORT); + * printf("X: %f\n", pos_x); * delay(20); * } * } * \endcode */ -gps_accel_s_t gps_get_accel(uint8_t port); +double gps_get_position_x(uint8_t port); /** - * Get the GPS's cartesian location relative to the center of turning/origin in meters. - * + * Gets the Y position in meters of the robot relative to the starting position. + * * This function uses the following values of errno when an error state is * reached: * ENXIO - The given value is not within the range of V5 ports (1-21). @@ -304,28 +411,29 @@ gps_accel_s_t gps_get_accel(uint8_t port); * * \param port * The V5 GPS port number from 1-21 - * \return A struct (gps_position_s_t) containing the X and Y values if the operation - * failed, setting errno. + * + * \return The Y position in meters. If the operation failed, + * returns PROS_ERR_F and errno is set. * * \b Example * \code * #define GPS_PORT 1 - * + * * void opcontrol() { - * gps_position_s_t pos; - * + * double pos_y; + * * while (true) { - * pos = gps_get_offset(GPS_PORT, x, y); - * screen_print(TEXT_MEDIUM, 1, "X Offset: %4d, Y Offset: %4d", pos.x, pos.y); + * pos_y = gps_get_position_y(GPS_PORT); + * printf("Y: %f\n", pos_y); * delay(20); * } * } * \endcode */ -gps_position_s_t gps_get_offset(uint8_t port); +double gps_get_position_y(uint8_t port); /** - * Sets the robot's location relative to the center of the field in meters. + * Gets the pitch, roll, and yaw of the GPS relative to the starting orientation. * * This function uses the following values of errno when an error state is * reached: @@ -335,31 +443,30 @@ gps_position_s_t gps_get_offset(uint8_t port); * * \param port * The V5 GPS port number from 1-21 - * \param xInitial - * Initial 4-Quadrant X Position, with (0,0) being at the center of the field (meters) - * \param yInitial - * Initial 4-Quadrant Y Position, with (0,0) being at the center of the field (meters) - * \param headingInitial - * Heading with 0 being north on the field, in degrees [0,360) going clockwise - * \return 1 if the operation was successful or PROS_ERR if the operation - * failed, setting errno. + * + * \return A struct (gps_orientation_s_t) containing values mentioned above. + * If the operation failed, all the structure's members are filled with + * PROS_ERR_F and errno is set. * * \b Example * \code * #define GPS_PORT 1 - * #define X_INITIAL -1.15 - * #define Y_INITIAL 1.45 - * #define HEADING_INITIAL 90 - * - * void initialize() { - * gps_set_position(GPS_PORT, X_INITIAL, Y_INITIAL, HEADING_INITIAL); + * + * void opcontrol() { + * gps_orientation_s_t orientation; + * + * while (true) { + * orientation = gps_get_orientation(GPS_PORT); + * printf("pitch: %f, roll: %f, yaw: %f\n", orientation.pitch, orientation.roll, orientation.yaw); + * delay(20); + * } * } * \endcode - */ -int32_t gps_set_position(uint8_t port, double xInitial, double yInitial, double headingInitial); +*/ +gps_orientation_s_t gps_get_orientation(uint8_t port); /** - * Set the GPS sensor's data rate in milliseconds, only applies to IMU on GPS. + * Gets the pitch of the robot in degrees relative to the starting oreintation. * * This function uses the following values of errno when an error state is * reached: @@ -369,28 +476,29 @@ int32_t gps_set_position(uint8_t port, double xInitial, double yInitial, double * * \param port * The V5 GPS port number from 1-21 - * \param rate - * Data rate in milliseconds (Minimum: 5 ms) - * \return 1 if the operation was successful or PROS_ERR if the operation - * failed, setting errno. + * + * \return The pitch in [0,360) degree values. If the operation failed, + * returns PROS_ERR_F and errno is set. * * \b Example * \code * #define GPS_PORT 1 - * #define GPS_DATA_RATE 5 - * - * void initialize() { - * gps_set_data_rate(GPS_PORT, GPS_DATA_RATE); + * + * void opcontrol() { + * double pitch; + * * while (true) { - * // Do something + * pitch = gps_get_pitch(GPS_PORT); + * printf("pitch: %f\n", pitch); + * delay(20); * } * } * \endcode */ -int32_t gps_set_data_rate(uint8_t port, uint32_t rate); +double gps_get_pitch(uint8_t port); /** - * Get the possible RMS (Root Mean Squared) error in meters for GPS position. + * Gets the roll of the robot in degrees relative to the starting oreintation. * * This function uses the following values of errno when an error state is * reached: @@ -400,25 +508,29 @@ int32_t gps_set_data_rate(uint8_t port, uint32_t rate); * * \param port * The V5 GPS port number from 1-21 - * - * \return Possible RMS (Root Mean Squared) error in meters for GPS position. - * If the operation failed, returns PROS_ERR_F and errno is set. + * + * \return The roll in [0,360) degree values. If the operation failed, + * returns PROS_ERR_F and errno is set. * * \b Example * \code * #define GPS_PORT 1 - * + * * void opcontrol() { - * double error; - * error = gps_get_error(GPS_PORT); - * screen_print(TEXT_MEDIUM, 1, "Error: %4d", error); + * double roll; + * + * while (true) { + * roll = gps_get_roll(GPS_PORT); + * printf("roll: %f\n", roll); + * delay(20); + * } * } * \endcode */ -double gps_get_error(uint8_t port); +double gps_get_roll(uint8_t port); /** - * Gets the position and roll, yaw, and pitch of the GPS. + * Gets the yaw of the robot in degrees relative to the starting oreintation. * * This function uses the following values of errno when an error state is * reached: @@ -428,29 +540,26 @@ double gps_get_error(uint8_t port); * * \param port * The V5 GPS port number from 1-21 - * - * \return A struct (gps_status_s_t) containing values mentioned above. - * If the operation failed, all the structure's members are filled with - * PROS_ERR_F and errno is set. + * + * \return The yaw in [0,360) degree values. If the operation failed, + * returns PROS_ERR_F and errno is set. * * \b Example * \code * #define GPS_PORT 1 - * + * * void opcontrol() { - * struct gps_status_s_t status; - * + * double yaw; + * * while (true) { - * status = gps_get_status(GPS_PORT); - * screen_print(TEXT_MEDIUM, 1, "x: %3f, y: %3f, pitch: %3f", status.x, status.y); - * screen_print(TEXT_MEDIUM, 2, "yaw: %3f, roll: %3f", status.pitch, status.yaw); - * screen_print(TEXT_MEDIUM, 3, "roll: %3f", status.roll); + * yaw = gps_get_yaw(GPS_PORT); + * printf("yaw: %f\n", yaw); * delay(20); * } * } * \endcode */ -gps_status_s_t gps_get_status(uint8_t port); +double gps_get_yaw(uint8_t port); /** * Get the heading in [0,360) degree values. @@ -470,12 +579,13 @@ gps_status_s_t gps_get_status(uint8_t port); * \b Example * \code * #define GPS_PORT 1 - * + * * void opcontrol() { * double heading; - * + * * while (true) { * heading = gps_get_heading(GPS_PORT); + * printf("heading: %f\n", heading); * delay(20); * } * } @@ -501,12 +611,13 @@ double gps_get_heading(uint8_t port); * \b Example * \code * #define GPS_PORT 1 - * + * * void opcontrol() { - * double heading; - * + * double heading_raw; + * * while (true) { - * heading = gps_get_heading_raw(GPS_PORT); + * heading_raw = gps_get_heading_raw(GPS_PORT); + * printf("heading_raw: %f\n", heading_raw); * delay(20); * } * } @@ -515,7 +626,7 @@ double gps_get_heading(uint8_t port); double gps_get_heading_raw(uint8_t port); /** - * Gets the GPS sensor's elapsed rotation value + * Get the GPS's raw gyroscope values * * This function uses the following values of errno when an error state is * reached: @@ -525,107 +636,119 @@ double gps_get_heading_raw(uint8_t port); * * \param port * The V5 GPS port number from 1-21 - * \return The elased heading in degrees. If the operation fails, returns - * PROS_ERR_F and errno is set. - * + * \return A struct (gps_gyro_s_t) containing values mentioned above. + * If the operation failed, all the + * structure's members are filled with PROS_ERR_F and errno is set. + * * \b Example * \code * #define GPS_PORT 1 - * + * * void opcontrol() { - * double elapsed_rotation; - * - * elapsed_rotation = gps_get_rotation(GPS_PORT); - * printf("Elapsed rotation: %3f", elapsed_rotation); + * gps_gyro_s_t gyro; + * + * while (true) { + * gyro = gps_get_gyro(GPS_PORT); + * printf("Gyro: %f %f %f\n", gyro.x, gyro.y, gyro.z); + * delay(20); + * } * } * \endcode */ -double gps_get_rotation(uint8_t port); +gps_gyro_s_t gps_get_gyro_rate(uint8_t port); /** - * Set the GPS sensor's rotation value to target value - * + * Get the GPS's raw gyroscope value in x-axis + * * This function uses the following values of errno when an error state is * reached: * ENXIO - The given value is not within the range of V5 ports (1-21). * ENODEV - The port cannot be configured as a GPS * EAGAIN - The sensor is still calibrating - * + * * \param port * The V5 GPS port number from 1-21 - * \param target - * Target rotation value to set rotation value to - * \return 1 if the operation was successful or PROS_ERR if the operation - * failed, setting errno. + * \return The raw gyroscope value in x-axis. If the operation fails, returns + * PROS_ERR_F and errno is set. * * \b Example * \code * #define GPS_PORT 1 - * + * * void opcontrol() { - * gps_set_rotation(GPS_PORT, 60); - * printf("Elapsed rotation: %3f", gps_get_rotation(GPS_PORT)); + * double gyro_x; + * + * while (true) { + * gyro_x = gps_get_gyro_x(GPS_PORT); + * printf("gyro_x: %f\n", gyro_x); + * delay(20); + * } * } * \endcode - */ -int32_t gps_set_rotation(uint8_t port, double target); +*/ +double gps_get_gyro_rate_x(uint8_t port); /** - * Tare the GPS sensor's rotation value - * + * Get the GPS's raw gyroscope value in y-axis + * * This function uses the following values of errno when an error state is * reached: * ENXIO - The given value is not within the range of V5 ports (1-21). * ENODEV - The port cannot be configured as a GPS * EAGAIN - The sensor is still calibrating - * + * * \param port * The V5 GPS port number from 1-21 - * \return 1 if the operation was successful or PROS_ERR if the operation - * failed, setting errno. + * \return The raw gyroscope value in y-axis. If the operation fails, returns + * PROS_ERR_F and errno is set. * * \b Example * \code * #define GPS_PORT 1 - * - * void initialize() { - * gps_tare_rotation(GPS_PORT); - * printf("Elapsed rotation: %3f", gps_get_rotation(GPS_PORT)); // should be 0 + * + * void opcontrol() { + * double gyro_y; + * + * while (true) { + * gyro_y = gps_get_gyro_y(GPS_PORT); + * printf("gyro_y: %f\n", gyro_y); + * delay(20); + * } * } * \endcode - */ -int32_t gps_tare_rotation(uint8_t port); +*/ +double gps_get_gyro_rate_y(uint8_t port); /** - * Get the GPS's raw gyroscope values - * + * Get the GPS's raw gyroscope value in z-axis + * * This function uses the following values of errno when an error state is * reached: * ENXIO - The given value is not within the range of V5 ports (1-21). * ENODEV - The port cannot be configured as a GPS * EAGAIN - The sensor is still calibrating - * + * * \param port * The V5 GPS port number from 1-21 - * \return The raw gyroscope values. If the operation failed, all the - * structure's members are filled with PROS_ERR_F and errno is set. + * \return The raw gyroscope value in z-axis. If the operation fails, returns + * PROS_ERR_F and errno is set. * * \b Example * \code * #define GPS_PORT 1 - * + * * void opcontrol() { - * struct gps_gyro_s_t gyro; - * + * double gyro_z; + * * while (true) { - * gyro = gps_get_gyro_rate(GPS_PORT); - * screen_print(TEXT_MEDIUM, 1, "gyroscope- x: %3f, y: %3f, z: %3f", gyro.x, gyro.y, gyro.z); + * gyro_z = gps_get_gyro_z(GPS_PORT); + * printf("gyro_z: %f\n", gyro_z); * delay(20); * } * } * \endcode - */ -gps_gyro_s_t gps_get_gyro_rate(uint8_t port); +*/ +double gps_get_gyro_rate_z(uint8_t port); /** * Get the GPS's raw accelerometer values @@ -638,26 +761,119 @@ gps_gyro_s_t gps_get_gyro_rate(uint8_t port); * * \param port * The V5 GPS's port number from 1-21 - * \return The raw accelerometer values. If the operation failed, all the + * \return A struct (gps_accel_s_t) containing values mentioned above. + * If the operation failed, all the * structure's members are filled with PROS_ERR_F and errno is set. * * \b Example * \code * #define GPS_PORT 1 - * + * * void opcontrol() { - * struct gps_accel_s_t accel; - * + * gps_accel_s_t accel; + * * while (true) { * accel = gps_get_accel(GPS_PORT); - * screen_print(TEXT_MEDIUM, 1, "accleration- x: %3f, y: %3f, z: %3f", accel.x, accel.y, accel.z); + * printf("X: %f, Y: %f, Z: %f\n", accel.x, accel.y, accel.z); + * delay(20); * } * } * \endcode */ gps_accel_s_t gps_get_accel(uint8_t port); -///@} +/** + * Get the GPS's raw accelerometer value in x-axis + * + * This function uses the following values of errno when an error state is + * reached: + * ENXIO - The given value is not within the range of V5 ports (1-21). + * ENODEV - The port cannot be configured as an GPS + * EAGAIN - The sensor is still calibrating + * + * \param port + * The V5 GPS's port number from 1-21 + * \return The raw accelerometer value in x-axis. If the operation fails, returns + * PROS_ERR_F and errno is set. + * + * \b Example + * \code + * #define GPS_PORT 1 + * + * void opcontrol() { + * double accel_x; + * + * while (true) { + * accel_x = gps_get_accel_x(GPS_PORT); + * printf("accel_x: %f\n", accel_x); + * delay(20); + * } + * } + * \endcode +*/ +double gps_get_accel_x(uint8_t port); + +/** + * Get the GPS's raw accelerometer value in y-axis + * + * This function uses the following values of errno when an error state is + * reached: + * ENXIO - The given value is not within the range of V5 ports (1-21). + * ENODEV - The port cannot be configured as an GPS + * EAGAIN - The sensor is still calibrating + * + * \param port + * The V5 GPS's port number from 1-21 + * \return The raw accelerometer value in y-axis. If the operation fails, returns + * PROS_ERR_F and errno is set. + * + * \b Example + * \code + * #define GPS_PORT 1 + * + * void opcontrol() { + * double accel_y; + * + * while (true) { + * accel_y = gps_get_accel_y(GPS_PORT); + * printf("accel_y: %f\n", accel_y); + * delay(20); + * } + * } + * \endcode +*/ +double gps_get_accel_y(uint8_t port); + +/** + * Get the GPS's raw accelerometer value in z-axis + * + * This function uses the following values of errno when an error state is + * reached: + * ENXIO - The given value is not within the range of V5 ports (1-21). + * ENODEV - The port cannot be configured as an GPS + * EAGAIN - The sensor is still calibrating + * + * \param port + * The V5 GPS's port number from 1-21 + * \return The raw accelerometer value in z-axis. If the operation fails, returns + * PROS_ERR_F and errno is set. + * + * \b Example + * \code + * #define GPS_PORT 1 + * + * void opcontrol() { + * double accel_z; + * + * while (true) { + * accel_z = gps_get_accel_z(GPS_PORT); + * printf("accel_z: %f\n", accel_z); + * delay(20); + * } + * } + * \endcode +*/ +double gps_get_accel_z(uint8_t port); #ifdef __cplusplus } diff --git a/include/pros/gps.hpp b/include/pros/gps.hpp index d081d619b..332e60b02 100644 --- a/include/pros/gps.hpp +++ b/include/pros/gps.hpp @@ -12,7 +12,7 @@ * This Source Code Form is subject to the terms of the Mozilla Public * License, v. 2.0. If a copy of the MPL was not distributed with this * file, You can obtain one at http://mozilla.org/MPL/2.0/. - * + * * \defgroup cpp-gps VEX GPS Sensor C API * \note For a pros-specific usage guide on the GPS, please check out our article [here.](@ref gps) */ @@ -25,8 +25,8 @@ #include #include -#include "pros/gps.h" #include "pros/device.hpp" +#include "pros/gps.h" namespace pros { inline namespace v5 { @@ -41,7 +41,6 @@ class Gps : public Device { */ public: - /** * Creates a GPS object for the given port. * @@ -59,7 +58,9 @@ class Gps : public Device { * \endcode * */ - explicit Gps(const std::uint8_t port) : Device(port, DeviceType::gps){}; + Gps(const std::uint8_t port) : Device(port, DeviceType::gps){}; + + Gps(const Device& device) : Gps(device.get_port()){}; /** * Creates a GPS object for the given port. @@ -85,7 +86,8 @@ class Gps : public Device { * \endcode * */ - explicit Gps(const std::uint8_t port, double xInitial, double yInitial, double headingInitial) : Device(port, DeviceType::gps){ + explicit Gps(const std::uint8_t port, double xInitial, double yInitial, double headingInitial) + : Device(port, DeviceType::gps) { pros::c::gps_set_position(port, xInitial, yInitial, headingInitial); }; @@ -105,19 +107,19 @@ class Gps : public Device { * \param yOffset * Cartesian 4-Quadrant Y offset from center of turning (meters) * - * \b Example: + * \b Example: * \code * pros::Gps gps(1, 1.30, 1.20); * \endcode * */ - explicit Gps(const std::uint8_t port, double xOffset, double yOffset) : Device(port, DeviceType::gps){ + explicit Gps(const std::uint8_t port, double xOffset, double yOffset) : Device(port, DeviceType::gps) { pros::c::gps_set_offset(port, xOffset, yOffset); }; /** * Creates a GPS object for the given port. - * + * * This function uses the following values of errno when an error state is * reached: * ENXIO - The given value is not within the range of V5 ports (1-21). @@ -143,8 +145,9 @@ class Gps : public Device { * \endcode * */ - explicit Gps(const std::uint8_t port, double xInitial, double yInitial, double headingInitial, double xOffset, double yOffset) - : Device(port, DeviceType::gps){ + explicit Gps(const std::uint8_t port, double xInitial, double yInitial, double headingInitial, double xOffset, + double yOffset) + : Device(port, DeviceType::gps) { pros::c::gps_initialize_full(port, xInitial, yInitial, headingInitial, xOffset, yOffset); }; @@ -221,34 +224,49 @@ class Gps : public Device { virtual std::int32_t set_offset(double xOffset, double yOffset) const; /** - * Get the GPS's cartesian location relative to the center of turning/origin in meters. - * - * This function uses the following values of errno when an error state is - * reached: - * ENXIO - The given value is not within the range of V5 ports (1-21). - * ENODEV - The port cannot be configured as a GPS - * EAGAIN - The sensor is still calibrating - * - * \param port - * The V5 GPS port number from 1-21 - * \return A struct (gps_position_s_t) containing the X and Y values if the operation - * failed, setting errno. - * - * \b Example - * \code - * #define GPS_PORT 1 - * - * void opcontrol() { - * gps_position_s_t pos; - * Gps gps(GPS_PORT); - * while (true) { - * pos = gps.get_offset(); - * screen_print(TEXT_MEDIUM, 1, "X Offset: %4d, Y Offset: %4d", pos.x, pos.y); - * delay(20); - * } - * } - * \endcode - */ + + * Gets all GPS sensors. + * + * \return A vector of Gps sensor objects. + * + * \b Example + * \code + * void opcontrol() { + * std::vector gps_all = pros::Gps::get_all_devices(); // All GPS sensors that are connected + * } + * \endcode + */ + static std::vector get_all_devices(); + + /** + * Get the GPS's cartesian location relative to the center of turning/origin in meters. + * + * This function uses the following values of errno when an error state is + * reached: + * ENXIO - The given value is not within the range of V5 ports (1-21). + * ENODEV - The port cannot be configured as a GPS + * EAGAIN - The sensor is still calibrating + * + * \param port + * The V5 GPS port number from 1-21 + * \return A struct (gps_position_s_t) containing the X and Y values if the operation + * failed, setting errno. + * + * \b Example + * \code + * #define GPS_PORT 1 + * + * void opcontrol() { + * gps_position_s_t pos; + * Gps gps(GPS_PORT); + * while (true) { + * pos = gps.get_offset(); + * screen_print(TEXT_MEDIUM, 1, "X Offset: %4d, Y Offset: %4d", pos.x, pos.y); + * delay(20); + * } + * } + * \endcode + */ virtual pros::gps_position_s_t get_offset() const; /** @@ -268,7 +286,7 @@ class Gps : public Device { * Heading with 0 being north on the field, in degrees [0,360) going clockwise * \return 1 if the operation was successful or PROS_ERR if the operation * failed, setting errno. - * + * * \b Example * \code * #define GPS_PORT 1 @@ -277,7 +295,7 @@ class Gps : public Device { * Gps gps(GPS_PORT); * gps.set_position(1.3, 1.4, 180); * while (true) { - * printf("X: %f, Y: %f, Heading: %f\n", gps.get_position().x, + * printf("X: %f, Y: %f, Heading: %f\n", gps.get_position().x, * gps.get_position().y, gps.get_position().heading); * delay(20); * } @@ -356,7 +374,7 @@ class Gps : public Device { * \return A struct (gps_status_s_t) containing values mentioned above. * If the operation failed, all the structure's members are filled with * PROS_ERR_F and errno is set. - * + * * \b Example * \code * #define GPS_PORT 1 @@ -365,15 +383,15 @@ class Gps : public Device { * Gps gps(GPS_PORT); * gps_status_s_t status; * while (true) { - * status = gps.get_status(); - * printf("X: %f, Y: %f, Heading: %f, Roll: %f, Pitch: %f, Yaw: %f\n", - * status.x, status.y, status.heading, status.roll, status.pitch, status.yaw); + * status = gps.get_position_and_orientation(); + * printf("X: %f, Y: %f, Roll: %f, Pitch: %f, Yaw: %f\n", + * status.x, status.y, status.roll, status.pitch, status.yaw); * delay(20); * } * } * \endcode */ - virtual pros::gps_status_s_t get_status() const; + virtual pros::gps_status_s_t get_position_and_orientation() const; /** * Gets the x and y position on the field of the GPS in meters. @@ -397,8 +415,7 @@ class Gps : public Device { * gps_position_s_t position; * while (true) { * position = gps.get_position(); - * printf("X: %f, Y: %f, Heading: %f\n", position.x, position.y, - * position.heading); + * printf("X: %f, Y: %f\n", position.x, position.y); * delay(20); * } * } @@ -406,6 +423,177 @@ class Gps : public Device { */ virtual pros::gps_position_s_t get_position() const; + /** + * Gets the X position in meters of the robot relative to the starting position. + * + * This function uses the following values of errno when an error state is + * reached: + * ENXIO - The given value is not within the range of V5 ports (1-21). + * ENODEV - The port cannot be configured as a GPS + * EAGAIN - The sensor is still calibrating + * + * \return The X position in meters. If the operation failed, + * returns PROS_ERR_F and errno is set. + * + * \b Example + * \code + * #define GPS_PORT 1 + * + * void opcontrol() { + * Gps gps(GPS_PORT); + * while(true) { + * double pos_x = gps.get_position_x(); + * printf("X: %f\n", pos_x); + * pros::delay(20); + * } + * } + * \endcode + */ + virtual double get_position_x() const; + + /** + * Gets the Y position in meters of the robot relative to the starting position. + * + * This function uses the following values of errno when an error state is + * reached: + * ENXIO - The given value is not within the range of V5 ports (1-21). + * ENODEV - The port cannot be configured as a GPS + * EAGAIN - The sensor is still calibrating + * + * \return The Y position in meters. If the operation failed, + * returns PROS_ERR_F and errno is set. + * + * \b Example + * \code + * #define GPS_PORT 1 + * + * void opcontrol() { + * Gps gps(GPS_PORT); + * while(true) { + * double pos_y = gps.get_position_y(); + * printf("Y: %f\n", pos_y); + * pros::delay(20); + * } + * } + * \endcode + */ + virtual double get_position_y() const; + + /** + * Gets the pitch, roll, and yaw of the GPS relative to the starting orientation. + * + * This function uses the following values of errno when an error state is + * reached: + * ENXIO - The given value is not within the range of V5 ports (1-21). + * ENODEV - The port cannot be configured as a GPS + * EAGAIN - The sensor is still calibrating + * + * \return A struct (gps_orientation_s_t) containing values mentioned above. + * If the operation failed, all the structure's members are filled with + * PROS_ERR_F and errno is set. + * + * \b Example + * \code + * #define GPS_PORT 1 + * + * void opcontrol() { + * Gps gps(GPS_PORT); + * gps_orientation_s_t orientation; + * while (true) { + * orientation = gps.get_orientation(); + * printf("pitch: %f, roll: %f, yaw: %f\n", orientation.pitch, + * orientation.roll, orientation.yaw); + * delay(20); + * } + * } + * \endcode + */ + virtual pros::gps_orientation_s_t get_orientation() const; + + /** + * Gets the pitch of the robot in degrees relative to the starting oreintation. + * + * This function uses the following values of errno when an error state is + * reached: + * ENXIO - The given value is not within the range of V5 ports (1-21). + * ENODEV - The port cannot be configured as a GPS + * EAGAIN - The sensor is still calibrating + * + * \return The pitch in [0,360) degree values. If the operation failed, + * returns PROS_ERR_F and errno is set. + * + * \b Example + * \code + * #define GPS_PORT 1 + * + * void opcontrol() { + * Gps gps(GPS_PORT); + * while(true) { + * double pitch = gps.get_pitch(); + * printf("pitch: %f\n", pitch); + * pros::delay(20); + * } + * } + * \endcode + */ + virtual double get_pitch() const; + + /** + * Gets the roll of the robot in degrees relative to the starting oreintation. + * + * This function uses the following values of errno when an error state is + * reached: + * ENXIO - The given value is not within the range of V5 ports (1-21). + * ENODEV - The port cannot be configured as a GPS + * EAGAIN - The sensor is still calibrating + * + * \return The roll in [0,360) degree values. If the operation failed, + * returns PROS_ERR_F and errno is set. + * + * \b Example + * \code + * #define GPS_PORT 1 + * + * void opcontrol() { + * Gps gps(GPS_PORT); + * while(true) { + * double roll = gps.get_roll(); + * printf("roll: %f\n", roll); + * pros::delay(20); + * } + * } + * \endcode + */ + virtual double get_roll() const; + + /** + * Gets the yaw of the robot in degrees relative to the starting oreintation. + * + * This function uses the following values of errno when an error state is + * reached: + * ENXIO - The given value is not within the range of V5 ports (1-21). + * ENODEV - The port cannot be configured as a GPS + * EAGAIN - The sensor is still calibrating + * + * \return The yaw in [0,360) degree values. If the operation failed, + * returns PROS_ERR_F and errno is set. + * + * \b Example + * \code + * #define GPS_PORT 1 + * + * void opcontrol() { + * Gps gps(GPS_PORT); + * while(true) { + * double yaw = gps.get_yaw(); + * printf("yaw: %f\n", yaw); + * pros::delay(20); + * } + * } + * \endcode + */ + virtual double get_yaw() const; + /** * Get the heading in [0,360) degree values. * @@ -464,7 +652,7 @@ class Gps : public Device { virtual double get_heading_raw() const; /** - * Gets the GPS sensor's elapsed rotation value + * Get the GPS's raw gyroscope value in z-axis * * This function uses the following values of errno when an error state is * reached: @@ -472,27 +660,27 @@ class Gps : public Device { * ENODEV - The port cannot be configured as a GPS * EAGAIN - The sensor is still calibrating * - * \return The elased heading in degrees. If the operation fails, returns + * \return The raw gyroscope value in z-axis. If the operation fails, returns * PROS_ERR_F and errno is set. * - * \b Example + \b Example * \code * #define GPS_PORT 1 * * void opcontrol() { * Gps gps(GPS_PORT); * while(true) { - double rotation = gps.get_rotation(); - * printf("Rotation: %f\n", rotation); + * double gyro_z = gps.get_gyro_z(); + * printf("gyro_z: %f\n", gyro_z); * pros::delay(20); * } * } * \endcode */ - virtual double get_rotation() const; + virtual pros::gps_gyro_s_t get_gyro_rate() const; /** - * Set the GPS sensor's rotation value to target value + * Get the GPS's raw gyroscope value in x-axis * * This function uses the following values of errno when an error state is * reached: @@ -500,10 +688,8 @@ class Gps : public Device { * ENODEV - The port cannot be configured as a GPS * EAGAIN - The sensor is still calibrating * - * \param target - * Target rotation value to set rotation value to - * \return 1 if the operation was successful or PROS_ERR if the operation - * failed, setting errno. + * \return The raw gyroscope value in x-axis. If the operation fails, returns + * PROS_ERR_F and errno is set. * * \b Example * \code @@ -511,18 +697,18 @@ class Gps : public Device { * * void opcontrol() { * Gps gps(GPS_PORT); - * double rotation = gps.set_rotation(90); * while(true) { - * printf("Rotation: %f\n", rotation); + * double gyro_x = gps.get_gyro_x(); + * printf("gyro_x: %f\n", gyro_x); * pros::delay(20); * } * } * \endcode */ - virtual std::int32_t set_rotation(double target) const; + virtual double get_gyro_rate_x() const; /** - * Tare the GPS sensor's rotation value + * Get the GPS's raw gyroscope value in y-axis * * This function uses the following values of errno when an error state is * reached: @@ -530,28 +716,27 @@ class Gps : public Device { * ENODEV - The port cannot be configured as a GPS * EAGAIN - The sensor is still calibrating * - * \return 1 if the operation was successful or PROS_ERR if the operation - * failed, setting errno. - * - * \b Example: + * \return The raw gyroscope value in y-axis. If the operation fails, returns + * PROS_ERR_F and errno is set. + * + * \b Example * \code * #define GPS_PORT 1 * * void opcontrol() { * Gps gps(GPS_PORT); - * gps.tare_rotation(); * while(true) { - * Should be around 0 on first call since it was tared. - * printf("Rotation: %f\n", rotation); + * double gyro_y = gps.get_gyro_y(); + * printf("gyro_y: %f\n", gyro_y); * pros::delay(20); * } * } * \endcode */ - virtual std::int32_t tare_rotation() const; + virtual double get_gyro_rate_y() const; /** - * Get the GPS's raw gyroscope values + * Get the GPS's raw gyroscope value in z-axis * * This function uses the following values of errno when an error state is * reached: @@ -559,24 +744,24 @@ class Gps : public Device { * ENODEV - The port cannot be configured as a GPS * EAGAIN - The sensor is still calibrating * - * \return The raw gyroscope values. If the operation failed, all the - * structure's members are filled with PROS_ERR_F and errno is set. + * \return The raw gyroscope value in z-axis. If the operation fails, returns + * PROS_ERR_F and errno is set. * - * \b Example + \b Example * \code * #define GPS_PORT 1 * * void opcontrol() { * Gps gps(GPS_PORT); * while(true) { - * pros::gps_gyro_s_t gyro = gps.get_gyro_rate(); - * printf("Gyro: %f, %f, %f\n", gyro.x, gyro.y, gyro.z); + * double gyro_z = gps.get_gyro_z(); + * printf("gyro_z: %f\n", gyro_z); * pros::delay(20); * } * } * \endcode - */ - virtual pros::gps_gyro_s_t get_gyro_rate() const; + */ + virtual double get_gyro_rate_z() const; /** * Get the GPS's raw accelerometer values @@ -587,18 +772,22 @@ class Gps : public Device { * ENODEV - The port cannot be configured as an GPS * EAGAIN - The sensor is still calibrating * - * \param port - * The V5 GPS's port number from 1-21 * \return The raw accelerometer values. If the operation failed, all the * structure's members are filled with PROS_ERR_F and errno is set. */ virtual pros::gps_accel_s_t get_accel() const; /** - * This is the overload for the << operator for printing to streams - * - * Prints in format: - * Gps [port: gps._port, x: (x position), y: (y position), heading: (gps heading), rotation: (gps rotation)] + * Get the GPS's raw accelerometer value in x-axis + * + * This function uses the following values of errno when an error state is + * reached: + * ENXIO - The given value is not within the range of V5 ports (1-21). + * ENODEV - The port cannot be configured as an GPS + * EAGAIN - The sensor is still calibrating + * + * \return The raw accelerometer value in x-axis. If the operation fails, returns + * PROS_ERR_F and errno is set. * * \b Example * \code @@ -607,43 +796,142 @@ class Gps : public Device { * void opcontrol() { * Gps gps(GPS_PORT); * while(true) { - * std::cout << gps << std::endl; + * double accel_x = gps.get_accel_x(); + * printf("accel_x: %f\n", accel_x); * pros::delay(20); * } * } * \endcode */ - friend std::ostream& operator<<(std::ostream& os, const pros::Gps& gps); + virtual double get_accel_x() const; -///@} -}; // Gps Class + /** + * Get the GPS's raw accelerometer value in y-axis + * + * This function uses the following values of errno when an error state is + * reached: + * ENXIO - The given value is not within the range of V5 ports (1-21). + * ENODEV - The port cannot be configured as an GPS + * EAGAIN - The sensor is still calibrating + * + * \return The raw accelerometer value in y-axis. If the operation fails, returns + * PROS_ERR_F and errno is set. + * + * \b Example + * \code + * #define GPS_PORT 1 + * + * void opcontrol() { + * Gps gps(GPS_PORT); + * while(true) { + * double accel_y = gps.get_accel_y(); + * printf("accel_y: %f\n", accel_y); + * pros::delay(20); + * } + * } + * \endcode + */ + virtual double get_accel_y() const; -namespace literals { /** - * Constructs a Gps object with the given port number - * + * Get the GPS's raw accelerometer value in z-axis + * + * This function uses the following values of errno when an error state is + * reached: + * ENXIO - The given value is not within the range of V5 ports (1-21). + * ENODEV - The port cannot be configured as an GPS + * EAGAIN - The sensor is still calibrating + * + * \return The raw accelerometer value in z-axis. If the operation fails, returns + * PROS_ERR_F and errno is set. + * * \b Example * \code - * using namespace literals; - * + * #define GPS_PORT 1 + * * void opcontrol() { - * pros::Gps gps = 1_gps; - * while (true) { - * pos = gps.get_position(); - * screen_print(TEXT_MEDIUM, 1, "X Position: %4d, Y Position: %4d", pos.x, pos.y); - * delay(20); - * } + * Gps gps(GPS_PORT); + * while(true) { + * double accel_z = gps.get_accel_z(); + * printf("accel_z: %f\n", accel_z); + * pros::delay(20); + * } + * } + * \endcode + */ + virtual double get_accel_z() const; + + /** + * This is the overload for the << operator for printing to streams + * + * Prints in format: + * Gps [port: gps._port, x: (x position), y: (y position), heading: (gps heading), rotation: (gps rotation)] + * + * \b Example + * \code + * #define GPS_PORT 1 + * + * void opcontrol() { + * Gps gps(GPS_PORT); + * while(true) { + * std::cout << gps << std::endl; + * pros::delay(20); + * } * } * \endcode */ - const pros::Gps operator""_gps(const unsigned long long int g); + friend std::ostream& operator<<(std::ostream& os, const pros::Gps& gps); + + /** + * Gets a gps sensor that is plugged in to the brain + * + * \note The first time this function is called it returns the gps sensor at the lowest port + * If this function is called multiple times, it will cycle through all the ports. + * For example, if you have 1 gps sensor on the robot + * this function will always return a gps sensor object for that port. + * If you have 2 gps sensors, all the odd numered calls to this function will return objects + * for the lower port number, + * all the even number calls will return gps objects for the higher port number + * + * + * This functions uses the following values of errno when an error state is + * reached: + * ENODEV - No gps sensor is plugged into the brain + * + * \return A gps object corresponding to a port that a gps sensor is connected to the brain + * If no gps sensor is plugged in, it returns a gps sensor on port PROS_ERR_BYTE + * + */ + static Gps get_gps(); + ///@} +}; // Gps Class + +namespace literals { +/** + * Constructs a Gps object with the given port number + * + * \b Example + * \code + * using namespace literals; + * + * void opcontrol() { + * pros::Gps gps = 1_gps; + * while (true) { + * pos = gps.get_position(); + * screen_print(TEXT_MEDIUM, 1, "X Position: %4d, Y Position: %4d", pos.x, pos.y); + * delay(20); + * } + * } + * \endcode + */ +const pros::Gps operator""_gps(const unsigned long long int g); } // namespace literals /// @brief /// Alias for Gps is GPS for user convenience. using GPS = Gps; -} // namespace v5 -} // namespace pros +} // namespace v5 +} // namespace pros #endif diff --git a/include/pros/imu.h b/include/pros/imu.h index bf724ab05..63cc73848 100644 --- a/include/pros/imu.h +++ b/include/pros/imu.h @@ -41,13 +41,25 @@ namespace pros { * @brief Indicates IMU status. */ typedef enum imu_status_e { + E_IMU_STATUS_READY = 0, // IMU is connected but not currently calibrating /** The IMU is calibrating */ - E_IMU_STATUS_CALIBRATING = 0x01, + E_IMU_STATUS_CALIBRATING = 1, /** Used to indicate that an error state was reached in the imu_get_status function,\ not that the IMU is necessarily in an error state */ E_IMU_STATUS_ERROR = 0xFF, } imu_status_e_t; +typedef enum imu_orientation_e { + E_IMU_Z_UP = 0, // IMU has the Z axis UP (VEX Logo facing DOWN) + E_IMU_Z_DOWN = 1, // IMU has the Z axis DOWN (VEX Logo facing UP) + E_IMU_X_UP = 2, // IMU has the X axis UP + E_IMU_X_DOWN = 3, // IMU has the X axis DOWN + E_IMU_Y_UP = 4, // IMU has the Y axis UP + E_IMU_Y_DOWN = 5, // IMU has the Y axis DOWN + E_IMU_ORIENTATION_ERROR = 0xFF // NOTE: used for returning an error from the get_physical_orientation function, not + // that the IMU is necessarily in an error state +} imu_orientation_e_t; + /** * \struct quaternion_s_t */ @@ -934,6 +946,21 @@ int32_t imu_set_roll(uint8_t port, double target); */ int32_t imu_set_yaw(uint8_t port, double target); +/** + * Returns the physical orientation of the IMU + * + * This function uses the following values of errno when an error state is + * reached: + * ENXIO - The given value is not within the range of V5 ports (1-21). + * ENODEV - The port cannot be configured as an Inertial Sensor + * + * \param port + * The V5 Inertial Sensor port number from 1-21 + * \returns The orientation of the Inertial Sensor or PROS_ERR if an error occured. + * + */ +imu_orientation_e_t imu_get_physical_orientation(uint8_t port); + /** @} */ /** @} */ diff --git a/include/pros/imu.hpp b/include/pros/imu.hpp index cb141eccb..192402203 100644 --- a/include/pros/imu.hpp +++ b/include/pros/imu.hpp @@ -19,10 +19,10 @@ #define _PROS_IMU_HPP_ #include +#include -#include "pros/imu.h" #include "pros/device.hpp" -#include +#include "pros/imu.h" namespace pros { /** @@ -40,8 +40,9 @@ namespace pros { */ enum class ImuStatus { + ready = 0, /** The IMU is calibrating */ - calibrating = 0x01, + calibrating = 19, /** Used to indicate that an error state was reached in the imu_get_status function,\ not that the IMU is necessarily in an error state */ error = 0xFF, @@ -56,7 +57,6 @@ class Imu : public Device { * \addtogroup cpp-imu * ///@{ */ - public: /** @@ -68,22 +68,47 @@ class Imu : public Device { * * \param port * The V5 Inertial Sensor port number from 1-21 - * + * * \b Example * \code * #define IMU_PORT 1 - * + * * void opcontrol() { * pros::Imu imu(IMU_PORT); - * + * * while (true) { * // Do something with the sensor data * } * } * \endcode */ - explicit Imu(const std::uint8_t port) : Device(port, DeviceType::imu) {}; + + + Imu(const std::uint8_t port) : Device(port, DeviceType::imu){}; + + Imu(const Device& device) : Imu(device.get_port()){}; + /** + * Gets a IMU sensor that is plugged in to the brain + * + * \note The first time this function is called it returns the IMU sensor at the lowest port + * If this function is called multiple times, it will cycle through all the ports. + * For example, if you have 1 IMU sensor on the robot + * this function will always return a IMU sensor object for that port. + * If you have 2 IMU sensors, all the odd numered calls to this function will return objects + * for the lower port number, + * all the even number calls will return IMU objects for the higher port number + * + * + * This functions uses the following values of errno when an error state is + * reached: + * ENODEV - No IMU sensor is plugged into the brain + * + * \return A IMU object corresponding to a port that a IMU sensor is connected to the brain + * If no IMU sensor is plugged in, it returns a IMU sensor on port PROS_ERR_BYTE + * + */ + static Imu get_imu(); /** * Calibrate IMU * @@ -103,12 +128,12 @@ class Imu : public Device { * Whether this function blocks during calibration. * \return 1 if the operation was successful or PROS_ERR if the operation * failed, setting errno. - * + * * \b Example * \code - * + * * #define IMU_PORT 1 - * + * * void opcontrol() { * pros::Imu imu(IMU_PORT); * imu.calibrate(); @@ -139,31 +164,48 @@ class Imu : public Device { * \param rate The data refresh interval in milliseconds * \return 1 if the operation was successful or PROS_ERR if the operation * failed, setting errno. - * + * * \b Example * \code - * + * * #define IMU_PORT 1 - * + * * void opcontrol() { * pros::Imu imu(IMU_PORT); - * + * * while (true) { * // Set the refresh rate to 5ms * std::int32_t status = imu.set_data_rate(5); * delay(20); - * + * * // Check if the operation was successful * if (status == PROS_ERR) { * // Do something with the error * } - * + * * // Do something with the sensor data * } * } * \endcode */ virtual std::int32_t set_data_rate(std::uint32_t rate) const; + + + /** + * Gets all IMU sensors. + * + * \return A vector of Imu sensor objects. + * + * \b Example + * \code + * void opcontrol() { + * std::vector imu_all = pros::Imu::get_all_devices(); // All IMU sensors that are connected + * } + * \endcode + */ + + static std::vector get_all_devices(); + /** * Get the total number of degrees the Inertial Sensor has spun about the z-axis * @@ -181,15 +223,15 @@ class Imu : public Device { * The V5 Inertial Sensor port number from 1-21 * \return The degree value or PROS_ERR_F if the operation failed, setting * errno. - * + * * \b Example * \code - * + * * #define IMU_PORT 1 - * + * * void opcontrol() { * pros::Imu imu(IMU_PORT); - * + * * while (true) { * // Get the total number of degrees the sensor has spun * printf("Total rotation: %f\n", imu.get_rotation()); @@ -217,15 +259,15 @@ class Imu : public Device { * The V5 Inertial Sensor port number from 1-21 * \return The degree value or PROS_ERR_F if the operation failed, setting * errno. - * + * * \b Example * \code - * + * * #define IMU_PORT 1 - * + * * void opcontrol() { * pros::Imu imu(IMU_PORT); - * + * * while (true) { * // Get the sensor's heading * printf("Heading: %f\n", imu.get_heading()); @@ -249,15 +291,15 @@ class Imu : public Device { * \return The quaternion representing the sensor's orientation. If the * operation failed, all the quaternion's members are filled with PROS_ERR_F and * errno is set. - * + * * \b Example * \code - * + * * #define IMU_PORT 1 - * + * * void opcontrol() { * pros::Imu imu(IMU_PORT); - * + * * while (true) { * // Get the sensor's quaternion * pros::quaternion_s_t quat = imu.get_quaternion(); @@ -282,15 +324,15 @@ class Imu : public Device { * \return The Euler angles representing the sensor's orientation. If the * operation failed, all the structure's members are filled with PROS_ERR_F and * errno is set. - * + * * \b Example * \code - * + * * #define IMU_PORT 1 - * + * * void opcontrol() { * pros::Imu imu(IMU_PORT); - * + * * while (true) { * // Get the sensor's Euler angles * pros::euler_s_t euler = imu.get_euler(); @@ -314,15 +356,15 @@ class Imu : public Device { * The V5 Inertial Sensor port number from 1-21 * \return The pitch angle, or PROS_ERR_F if the operation failed, setting * errno. - * + * * \b Example * \code - * + * * #define IMU_PORT 1 - * + * * void opcontrol() { * pros::Imu imu(IMU_PORT); - * + * * while (true) { * // Get the sensor's pitch * printf("Pitch: %f\n", imu.get_pitch()); @@ -344,15 +386,15 @@ class Imu : public Device { * \param port * The V5 Inertial Sensor port number from 1-21 * \return The roll angle, or PROS_ERR_F if the operation failed, setting errno. - * + * * \b Example * \code - * + * * #define IMU_PORT 1 - * + * * void opcontrol() { * pros::Imu imu(IMU_PORT); - * + * * while (true) { * // Get the sensor's roll * printf("Roll: %f\n", imu.get_roll()); @@ -374,15 +416,15 @@ class Imu : public Device { * \param port * The V5 Inertial Sensor port number from 1-21 * \return The yaw angle, or PROS_ERR_F if the operation failed, setting errno. - * + * * \b Example * \code - * + * * #define IMU_PORT 1 - * + * * void opcontrol() { * pros::Imu imu(IMU_PORT); - * + * * while (true) { * // Get the sensor's yaw * printf("Yaw: %f\n", imu.get_yaw()); @@ -405,15 +447,15 @@ class Imu : public Device { * The V5 Inertial Sensor port number from 1-21 * \return The raw gyroscope values. If the operation failed, all the * structure's members are filled with PROS_ERR_F and errno is set. - * + * * \b Example * \code - * + * * #define IMU_PORT 1 - * + * * void opcontrol() { * pros::Imu imu(IMU_PORT); - * + * * while (true) { * // Get the sensor's raw gyroscope values * pros::imu_gyro_s_t gyro = imu.get_gyro_rate(); @@ -437,22 +479,22 @@ class Imu : public Device { * The V5 Inertial Sensor port number from 1-21 * \return 1 if the operation was successful or PROS_ERR if the operation * failed, setting errno. - * + * * \b Example * \code - * + * * #define IMU_PORT 1 - * + * * void opcontrol() { * pros::Imu imu(IMU_PORT); - * + * * while (true) { * // Set the sensor's rotation value to 10 * imu.set_rotation(10); * delay(20); - * + * * // Do something with sensor - * + * * // Reset the sensor's rotation value to 0 * imu.tare_rotation(); * delay(20); @@ -474,22 +516,22 @@ class Imu : public Device { * The V5 Inertial Sensor port number from 1-21 * \return 1 if the operation was successful or PROS_ERR if the operation * failed, setting errno. - * + * * \b Example * \code - * + * * #define IMU_PORT 1 - * + * * void opcontrol() { * pros::Imu imu(IMU_PORT); - * + * * while (true) { * // Set the sensor's heading value to 10 * imu.set_heading(10); * delay(20); - * + * * // Do something with sensor - * + * * // Reset the sensor's heading value to 0 * imu.tare_heading(); * delay(20); @@ -511,22 +553,22 @@ class Imu : public Device { * The V5 Inertial Sensor port number from 1-21 * \return 1 if the operation was successful or PROS_ERR if the operation * failed, setting errno. - * + * * \b Example * \code - * + * * #define IMU_PORT 1 - * + * * void opcontrol() { * pros::Imu imu(IMU_PORT); - * + * * while (true) { * // Set the sensor's pitch value to 10 * imu.set_pitch(10); * delay(20); - * + * * // Do something with sensor - * + * * // Reset the sensor's pitch value to 0 * imu.tare_pitch(); * delay(20); @@ -548,22 +590,22 @@ class Imu : public Device { * The V5 Inertial Sensor port number from 1-21 * \return 1 if the operation was successful or PROS_ERR if the operation * failed, setting errno. - * + * * \b Example * \code - * + * * #define IMU_PORT 1 - * + * * void opcontrol() { * pros::Imu imu(IMU_PORT); - * + * * while (true) { * // Set the sensor's yaw value to 10 * imu.set_yaw(10); * delay(20); - * + * * // Do something with sensor - * + * * // Reset the sensor's yaw value to 0 * imu.tare_yaw(); * delay(20); @@ -585,22 +627,22 @@ class Imu : public Device { * The V5 Inertial Sensor port number from 1-21 * \return 1 if the operation was successful or PROS_ERR if the operation * failed, setting errno. - * + * * \b Example * \code - * + * * #define IMU_PORT 1 - * + * * void opcontrol() { * pros::Imu imu(IMU_PORT); - * + * * while (true) { * // Set the sensor's roll value to 10 * imu.set_roll(10); * delay(20); - * + * * // Do something with sensor - * + * * // Reset the sensor's roll value to 0 * imu.tare_roll(); * delay(20); @@ -622,15 +664,15 @@ class Imu : public Device { * The V5 Inertial Sensor port number from 1-21 * \return 1 if the operation was successful or PROS_ERR if the operation * failed, setting errno. - * + * * \b Example * \code - * + * * #define IMU_PORT 1 - * + * * void opcontrol() { * pros::Imu imu(IMU_PORT); - * + * * while (true) { * // Reset all values of the sensor to 0 * imu.tare(); @@ -653,22 +695,22 @@ class Imu : public Device { * The V5 Inertial Sensor port number from 1-21 * \return 1 if the operation was successful or PROS_ERR if the operation * failed, setting errno. - * + * * \b Example * \code - * + * * #define IMU_PORT 1 - * + * * void opcontrol() { * pros::Imu imu(IMU_PORT); - * + * * while (true) { * // Reset all euler values of the sensor to 0 * imu.tare_euler(); * delay(20); * } * } - * \endcode + * \endcode */ virtual std::int32_t tare_euler() const; /** @@ -687,20 +729,20 @@ class Imu : public Device { * Target value for the heading value to be set to * \return 1 if the operation was successful or PROS_ERR if the operation * failed, setting errno. - * + * * \b Example * \code - * + * * #define IMU_PORT 1 - * + * * void opcontrol() { * pros::Imu imu(IMU_PORT); - * + * * while (true) { * // Set the sensor's heading value to 10 * imu.set_heading(10); * delay(20); - * + * * // Do something with sensor * } * } @@ -722,20 +764,20 @@ class Imu : public Device { * Target value for the rotation value to be set to * \return 1 if the operation was successful or PROS_ERR if the operation * failed, setting errno. - * + * * \b Example * \code - * + * * #define IMU_PORT 1 - * + * * void opcontrol() { * pros::Imu imu(IMU_PORT); - * + * * while (true) { * // Set the sensor's rotation value to 10 * imu.set_rotation(10); * delay(20); - * + * * // Do something with sensor * } * } @@ -758,20 +800,20 @@ class Imu : public Device { * Target value for yaw value to be set to * \return 1 if the operation was successful or PROS_ERR if the operation * failed, setting errno. - * + * * \b Example * \code - * + * * #define IMU_PORT 1 - * + * * void opcontrol() { * pros::Imu imu(IMU_PORT); - * + * * while (true) { * // Set the sensor's yaw value to 10 * imu.set_yaw(10); * delay(20); - * + * * // Do something with sensor * } * } @@ -793,20 +835,20 @@ class Imu : public Device { * Target value for the pitch value to be set to * \return 1 if the operation was successful or PROS_ERR if the operation * failed, setting errno. - * + * * \b Example * \code - * + * * #define IMU_PORT 1 - * + * * void opcontrol() { * pros::Imu imu(IMU_PORT); - * + * * while (true) { * // Set the sensor's pitch value to 10 * imu.set_pitch(10); * delay(20); - * + * * // Do something with sensor * } * } @@ -829,20 +871,20 @@ class Imu : public Device { * Target euler values for the euler values to be set to * \return 1 if the operation was successful or PROS_ERR if the operation * failed, setting errno. - * + * * \b Example * \code - * + * * #define IMU_PORT 1 - * + * * void opcontrol() { * pros::Imu imu(IMU_PORT); - * + * * while (true) { * // Set the sensor's roll value to 100 * imu.set_roll(100); * delay(20); - * + * * // Do something with sensor * } * } @@ -865,20 +907,20 @@ class Imu : public Device { * Target euler values for the euler values to be set to * \return 1 if the operation was successful or PROS_ERR if the operation * failed, setting errno. - * + * * \b Example * \code - * + * * #define IMU_PORT 1 - * + * * void opcontrol() { * pros::Imu imu(IMU_PORT); - * + * * while (true) { * // Set the sensor's euler values to 50 * imu.set_euler(50); * delay(20); - * + * * // Do something with sensor * } * } @@ -898,25 +940,25 @@ class Imu : public Device { * The V5 Inertial Sensor port number from 1-21 * \return The raw accelerometer values. If the operation failed, all the * structure's members are filled with PROS_ERR_F and errno is set. - * + * * \b Example * \code - * + * * #define IMU_PORT 1 - * + * * void opcontrol() { * pros::Imu imu(IMU_PORT); - * + * * while (true) { * // Get the sensor's raw accelerometer values * pros::imu_accel_s_t accel = imu.get_accel(); * printf("x: %f, y: %f, z: %f\n", accel.x, accel.y, accel.z); * delay(20); - * + * * // Do something with sensor * } * } - * \endcode + * \endcode */ virtual pros::imu_accel_s_t get_accel() const; /** @@ -932,21 +974,21 @@ class Imu : public Device { * The V5 Inertial Sensor port number from 1-21 * \return The Inertial Sensor's status code, or PROS_ERR if the operation * failed, setting errno. - * + * * \b Example * \code - * + * * #define IMU_PORT 1 - * + * * void opcontrol() { * pros::Imu imu(IMU_PORT); - * + * * while (true) { * // Get the sensor's status * pros::ImuStatus status = imu.get_status(); * cout << "Status: " << status << endl; * delay(20); - * + * * // Do something with sensor * } * } @@ -958,38 +1000,52 @@ class Imu : public Device { * * \return true if the V5 Inertial Sensor is calibrating or false * false if it is not. - * + * * \b Example * \code - * + * * #define IMU_PORT 1 - * + * * void opcontrol() { * pros::Imu imu(IMU_PORT); - * + * * while (true) { * // Calibrate the sensor * imu.calibrate(); * delay(20); - * + * * // Check if the sensor is calibrating * if (imu.is_calibrating()) { * printf("Calibrating...\n"); * } - * + * * // Do something with sensor * } * } * \endcode */ virtual bool is_calibrating() const; + /** + * Returns the physical orientation of the IMU + * + * This function uses the following values of errno when an error state is + * reached: + * ENXIO - The given value is not within the range of V5 ports (1-21). + * ENODEV - The port cannot be configured as an Inertial Sensor + * + * \param port + * The V5 Inertial Sensor port number from 1-21 + * \returns The physical orientation of the Inertial Sensor or PROS_ERR if an error occured. + * + */ + virtual imu_orientation_e_t get_physical_orientation() const; /** - * This is the overload for the << operator for printing to streams - * - * Prints in format(this below is all in one line with no new line): - * Imu [port: imu._port, rotation: (rotation), heading: (heading), - * pitch: (pitch angle), roll: (roll angle), yaw: (yaw angle), + * This is the overload for the << operator for printing to streams + * + * Prints in format(this below is all in one line with no new line): + * Imu [port: imu._port, rotation: (rotation), heading: (heading), + * pitch: (pitch angle), roll: (roll angle), yaw: (yaw angle), * gyro rate: {x,y,z}, get accel: {x,y,z}, calibrating: (calibrating boolean)] */ friend std::ostream& operator<<(std::ostream& os, const pros::Imu& imu); @@ -998,6 +1054,19 @@ class Imu : public Device { }; namespace literals { +/** + * Constructs a Imu from a literal ending in _imu via calling the constructor + * + * \return a pros::Imu for the corresponding port + * + * \b Example + * \code + * using namespace pros::literals; + * void opcontrol() { + * pros::Imu imu = 2_imu; //Makes an IMU object on port 2 + * } + * \endcode + */ const pros::Imu operator"" _imu(const unsigned long long int i); } // namespace literals diff --git a/include/pros/link.h b/include/pros/link.h index bd02a17e1..9cd6eeade 100644 --- a/include/pros/link.h +++ b/include/pros/link.h @@ -41,9 +41,9 @@ namespace pros { * \brief Enum for the type of link (TX or RX) */ typedef enum link_type_e { - E_LINK_RECEIVER = 0, ///< Indicates that the radio is a receiver. + E_LINK_RECIEVER = 0, ///< Indicates that the radio is a reciever. E_LINK_TRANSMITTER, ///< Indicates that the link is a transmitter. - E_LINK_RX = E_LINK_RECEIVER, ///< Alias for E_LINK_RECEIVER + E_LINK_RX = E_LINK_RECIEVER, ///< Alias for E_LINK_RECIEVER E_LINK_TX = E_LINK_TRANSMITTER ///< Alias for E_LINK_TRANSMITTER } link_type_e_t; diff --git a/include/pros/link.hpp b/include/pros/link.hpp index 45d98df8e..68bb5e517 100644 --- a/include/pros/link.hpp +++ b/include/pros/link.hpp @@ -56,7 +56,7 @@ class Link : public Device { * with the transmitter having double the transmitting bandwidth as the receiving * end (1040 bytes/s vs 520 bytes/s). * \param ov - * Indicates if the radio on the given port needs vexlink to override the controller radio + * Indicates if the radio on the given port needs vexlink to override the controller radio. Defualts to True. * * \return PROS_ERR if initialization fails, 1 if the initialization succeeds. * @@ -65,7 +65,7 @@ class Link : public Device { * pros::Link link(1, "my_link", pros::E_LINK_TX); * \endcode */ - explicit Link(const std::uint8_t port, const std::string link_id, link_type_e_t type, bool ov = false); + explicit Link(const std::uint8_t port, const std::string link_id, link_type_e_t type, bool ov = true); /** * Checks if a radio link on a port is active or not. diff --git a/include/pros/misc.h b/include/pros/misc.h index d0ad6764b..677dad811 100644 --- a/include/pros/misc.h +++ b/include/pros/misc.h @@ -14,7 +14,7 @@ * This Source Code Form is subject to the terms of the Mozilla Public * License, v. 2.0. If a copy of the MPL was not distributed with this * file, You can obtain one at http://mozilla.org/MPL/2.0/. - * + * * \defgroup c-misc Miscellaneous C API * \note Additional example code for this module can be found in its [Tutorial.](@ref controller) */ @@ -38,9 +38,16 @@ /// \name V5 Competition //@{ -#define COMPETITION_DISABLED (1 << 0) +/*#define COMPETITION_DISABLED (1 << 0) #define COMPETITION_AUTONOMOUS (1 << 1) #define COMPETITION_CONNECTED (1 << 2) +#define COMPETITION_SYSTEM (1 << 3)*/ +typedef enum { + COMPETITION_DISABLED = 1 << 0, + COMPETITION_CONNECTED = 1 << 2, + COMPETITION_AUTONOMOUS = 1 << 1, + COMPETITION_SYSTEM = 1 << 3, +} competition_status; #ifdef __cplusplus extern "C" { @@ -54,7 +61,7 @@ namespace c { * * \return The competition control status as a mask of bits with * COMPETITION_{ENABLED,AUTONOMOUS,CONNECTED}. - * + * * \b Example * \code * void initialize() { @@ -67,17 +74,11 @@ namespace c { */ uint8_t competition_get_status(void); -#ifdef __cplusplus -} -} -} -#endif - /** * \fn competition_is_disabled() - * + * * \return True if the V5 Brain is disabled, false otherwise. - * + * * \b Example * \code * void my_task_fn(void* ignore) { @@ -85,17 +86,17 @@ uint8_t competition_get_status(void); * // Run competition tasks (like Lift Control or similar) * } * } - * + * * void initialize() { * task_t my_task = task_create(my_task_fn, NULL, TASK_PRIO_DEFAULT, TASK_STACK_DEPTH_DEFAULT, "My Task"); * } * \endcode */ -#define competition_is_disabled() ((competition_get_status() & COMPETITION_DISABLED) != 0) +uint8_t competition_is_disabled(void); /** * \return True if the V5 Brain is connected to competition control, false otherwise. - * + * * \b Example * \code * void initialize() { @@ -106,11 +107,11 @@ uint8_t competition_get_status(void); * } * \endcode */ -#define competition_is_connected() ((competition_get_status() & COMPETITION_CONNECTED) != 0) +uint8_t competition_is_connected(void); /** * \return True if the V5 Brain is in autonomous mode, false otherwise. - * + * * \b Example * \code * void my_task_fn(void* ignore) { @@ -122,14 +123,46 @@ uint8_t competition_get_status(void); * // Run whatever code is desired to just execute in autonomous * } * } - * + * * void initialize() { * task_t my_task = task_create(my_task_fn, NULL, TASK_PRIO_DEFAULT, TASK_STACK_DEPTH_DEFAULT, "My Task"); * } * \endcode */ -#define competition_is_autonomous() ((competition_get_status() & COMPETITION_AUTONOMOUS) != 0) +uint8_t competition_is_autonomous(void); + +/** + * \return True if the V5 Brain is connected to VEXnet Field Controller, false otherwise. + * + * \b Example + * \code + * void initialize() { + * if (competition_is_field()) { + * // connected to VEXnet Field Controller + * } + * } + * \endcode + */ +uint8_t competition_is_field(void); +/** + * \return True if the V5 Brain is connected to VEXnet Competition Switch, false otherwise. + * + * \b Example + * \code + * void initialize() { + * if (competition_is_switch()) { + * // connected to VEXnet Competition Switch + * } + * } + */ +uint8_t competition_is_switch(void); + +#ifdef __cplusplus +} +} +} +#endif ///@} /// \name V5 Controller @@ -143,22 +176,23 @@ namespace pros { * \enum */ typedef enum { - ///The master controller. + /// The master controller. E_CONTROLLER_MASTER = 0, - ///The partner controller. - E_CONTROLLER_PARTNER } controller_id_e_t; + /// The partner controller. + E_CONTROLLER_PARTNER +} controller_id_e_t; /** * \enum */ typedef enum { - ///The horizontal axis of the controller’s left analog stick. + /// The horizontal axis of the controller’s left analog stick. E_CONTROLLER_ANALOG_LEFT_X = 0, - ///The vertical axis of the controller’s left analog stick. + /// The vertical axis of the controller’s left analog stick. E_CONTROLLER_ANALOG_LEFT_Y, - ///The horizontal axis of the controller’s right analog stick. + /// The horizontal axis of the controller’s right analog stick. E_CONTROLLER_ANALOG_RIGHT_X, - ///The vertical axis of the controller’s right analog stick. + /// The vertical axis of the controller’s right analog stick. E_CONTROLLER_ANALOG_RIGHT_Y } controller_analog_e_t; @@ -166,29 +200,29 @@ typedef enum { * \enum */ typedef enum { - ///The first trigger on the left side of the controller. + /// The first trigger on the left side of the controller. E_CONTROLLER_DIGITAL_L1 = 6, - ///The second trigger on the left side of the controller. + /// The second trigger on the left side of the controller. E_CONTROLLER_DIGITAL_L2, - ///The first trigger on the right side of the controller. + /// The first trigger on the right side of the controller. E_CONTROLLER_DIGITAL_R1, - ///The second trigger on the right side of the controller. + /// The second trigger on the right side of the controller. E_CONTROLLER_DIGITAL_R2, - ///The up arrow on the left arrow pad of the controller. + /// The up arrow on the left arrow pad of the controller. E_CONTROLLER_DIGITAL_UP, - ///The down arrow on the left arrow pad of the controller. + /// The down arrow on the left arrow pad of the controller. E_CONTROLLER_DIGITAL_DOWN, - ///The left arrow on the left arrow pad of the controller. + /// The left arrow on the left arrow pad of the controller. E_CONTROLLER_DIGITAL_LEFT, - ///The right arrow on the left arrow pad of the controller. + /// The right arrow on the left arrow pad of the controller. E_CONTROLLER_DIGITAL_RIGHT, - ///The ‘X’ button on the right button pad of the controller. + /// The ‘X’ button on the right button pad of the controller. E_CONTROLLER_DIGITAL_X, - ///The ‘B’ button on the right button pad of the controller. + /// The ‘B’ button on the right button pad of the controller. E_CONTROLLER_DIGITAL_B, - ///The ‘Y’ button on the right button pad of the controller. + /// The ‘Y’ button on the right button pad of the controller. E_CONTROLLER_DIGITAL_Y, - ///The ‘A’ button on the right button pad of the controller. + /// The ‘A’ button on the right button pad of the controller. E_CONTROLLER_DIGITAL_A } controller_digital_e_t; @@ -235,26 +269,27 @@ typedef enum { #endif /** - * \def Given an id and a port, this macro sets the port variable based on the id and allows the mutex to take that port. - * + * \def Given an id and a port, this macro sets the port variable based on the id and allows the mutex to take that + * port. + * * \returns error (in the function/scope it's in) if the controller failed to connect or an invalid id is given. -*/ + */ #define CONTROLLER_PORT_MUTEX_TAKE(id, port) \ - switch (id) { \ - case E_CONTROLLER_MASTER: \ - port = V5_PORT_CONTROLLER_1; \ - break; \ - case E_CONTROLLER_PARTNER: \ - port = V5_PORT_CONTROLLER_2; \ - break; \ - default: \ - errno = EINVAL; \ - return PROS_ERR; \ - } \ - if (!internal_port_mutex_take(port)) { \ - errno = EACCES; \ - return PROS_ERR; \ - } \ + switch (id) { \ + case E_CONTROLLER_MASTER: \ + port = V5_PORT_CONTROLLER_1; \ + break; \ + case E_CONTROLLER_PARTNER: \ + port = V5_PORT_CONTROLLER_2; \ + break; \ + default: \ + errno = EINVAL; \ + return PROS_ERR; \ + } \ + if (!internal_port_mutex_take(port)) { \ + errno = EACCES; \ + return PROS_ERR; \ + } #ifdef __cplusplus namespace c { @@ -274,7 +309,7 @@ namespace c { * Must be one of CONTROLLER_MASTER or CONTROLLER_PARTNER * * \return 1 if the controller is connected, 0 otherwise - * + * * \b Example * \code * void initialize() { @@ -306,7 +341,7 @@ int32_t controller_is_connected(controller_id_e_t id); * * \return The current reading of the analog channel: [-127, 127]. * If the controller was not connected, then 0 is returned - * + * * \b Example * \code * void opcontrol() { @@ -333,7 +368,7 @@ int32_t controller_get_analog(controller_id_e_t id, controller_analog_e_t channe * Must be one of E_CONTROLLER_MASTER or E_CONTROLLER_PARTNER * * \return The controller's battery capacity - * + * * \b Example * \code * void initialize() { @@ -357,7 +392,7 @@ int32_t controller_get_battery_capacity(controller_id_e_t id); * Must be one of E_CONTROLLER_MASTER or E_CONTROLLER_PARTNER * * \return The controller's battery level - * + * * \b Example * \code * void initialize() { @@ -385,7 +420,7 @@ int32_t controller_get_battery_level(controller_id_e_t id); * * \return 1 if the button on the controller is pressed. * If the controller was not connected, then 0 is returned - * + * * \b Example * \code * void opcontrol() { @@ -430,7 +465,7 @@ int32_t controller_get_digital(controller_id_e_t id, controller_digital_e_t butt * * \return 1 if the button on the controller is pressed and had not been pressed * the last time this function was called, 0 otherwise. - * + * * \b Example * \code * void opcontrol() { @@ -438,7 +473,7 @@ int32_t controller_get_digital(controller_id_e_t id, controller_digital_e_t butt * if (controller_get_digital_new_press(E_CONTROLLER_MASTER, E_CONTROLLER_DIGITAL_A)) { * // Toggle pneumatics or other similar actions * } - * + * * delay(2); * } * } @@ -473,7 +508,7 @@ int32_t controller_get_digital_new_press(controller_id_e_t id, controller_digita * * \return 1 if the operation was successful or PROS_ERR if the operation * failed, setting errno. - * + * * \b Example * \code * void opcontrol() { @@ -516,7 +551,7 @@ int32_t controller_print(controller_id_e_t id, uint8_t line, uint8_t col, const * * \return 1 if the operation was successful or PROS_ERR if the operation * failed, setting errno. - * + * * \b Example * \code * void opcontrol() { @@ -554,7 +589,7 @@ int32_t controller_set_text(controller_id_e_t id, uint8_t line, uint8_t col, con * * \return 1 if the operation was successful or PROS_ERR if the operation * failed, setting errno. - * + * * \b Example * \code * void opcontrol() { @@ -585,7 +620,7 @@ int32_t controller_clear_line(controller_id_e_t id, uint8_t line); * * \return 1 if the operation was successful or PROS_ERR if the operation * failed, setting errno. - * + * * \b Example * \code * void opcontrol() { @@ -619,7 +654,7 @@ int32_t controller_clear(controller_id_e_t id); * * \return 1 if the operation was successful or PROS_ERR if the operation * failed, setting errno. - * + * * \b Example * \code * void opcontrol() { @@ -645,7 +680,7 @@ int32_t controller_rumble(controller_id_e_t id, const char* rumble_pattern); * EACCES - Another resource is currently trying to access the battery port. * * \return The current voltage of the battery - * + * * \b Example * \code * void initialize() { @@ -663,7 +698,7 @@ int32_t battery_get_voltage(void); * EACCES - Another resource is currently trying to access the battery port. * * \return The current current of the battery - * + * * \b Example * \code * void initialize() { @@ -681,7 +716,7 @@ int32_t battery_get_current(void); * EACCES - Another resource is currently trying to access the battery port. * * \return The current temperature of the battery - * + * * \b Example * \code * void initialize() { @@ -699,7 +734,7 @@ double battery_get_temperature(void); * EACCES - Another resource is currently trying to access the battery port. * * \return The current capacity of the battery - * + * * \b Example * \code * void initialize() { @@ -713,7 +748,7 @@ double battery_get_capacity(void); * Checks if the SD card is installed. * * \return 1 if the SD card is installed, 0 otherwise - * + * * \b Example * \code * void opcontrol() { @@ -723,6 +758,49 @@ double battery_get_capacity(void); */ int32_t usd_is_installed(void); +/** + * Lists the files in a directory specified by the path + * Puts the list of file names (NOT DIRECTORIES) into the buffer seperated by newlines + * + * This function uses the following values of errno when an error state is + * reached: + * + * EIO - Hard error occured in the low level disk I/O layer + * EINVAL - file or directory is invalid, or length is invalid + * EBUSY - THe physical drinve cannot work + * ENOENT - cannot find the path or file + * EINVAL - the path name format is invalid + * EACCES - Access denied or directory full + * EEXIST - Access denied + * EROFS - SD card is write protected + * ENXIO - drive number is invalid or not a FAT32 drive + * ENOBUFS - drive has no work area + * ENFILE - too many open files + * + * + * + * \note use a path of "\" to list the files in the main directory NOT "/usd/" + * DO NOT PREPEND YOUR PATHS WITH "/usd/" + * + * \return 1 on success or PROS_ERR on failure setting errno + * + * \b Example + * \code + * void opcontrol() { + * char* test = (char*) malloc(128); + * pros::c::usd_list_files("/", test, 128); + * pros::delay(200); + * printf("%s\n", test); //Prints the file names in the root directory seperated by newlines + * pros::delay(100); + * pros::c::usd_list_files("/test", test, 128); + * pros::delay(200); + * printf("%s\n", test); //Prints the names of files in the folder named test seperated by newlines + * pros::delay(100); + * } + * \endcode + */ +int32_t usd_list_files(const char* path, char* buffer, int32_t len); + /******************************************************************************/ /** Date and Time **/ /******************************************************************************/ @@ -731,16 +809,16 @@ extern const char* baked_date; extern const char* baked_time; typedef struct { - uint16_t year; // Year - 1980 + uint16_t year; // Year - 1980 uint8_t day; - uint8_t month; // 1 = January + uint8_t month; // 1 = January } date_s_t; typedef struct { uint8_t hour; uint8_t min; uint8_t sec; - uint8_t sec_hund; // hundredths of a second + uint8_t sec_hund; // hundredths of a second } time_s_t; ///@} @@ -749,7 +827,7 @@ typedef struct { #ifdef __cplusplus } -} // namespace pros +} // namespace pros } #endif diff --git a/include/pros/misc.hpp b/include/pros/misc.hpp index 8ade055ad..efa4d1d87 100644 --- a/include/pros/misc.hpp +++ b/include/pros/misc.hpp @@ -14,7 +14,7 @@ * This Source Code Form is subject to the terms of the Mozilla Public * License, v. 2.0. If a copy of the MPL was not distributed with this * file, You can obtain one at http://mozilla.org/MPL/2.0/. - * + * * \defgroup cpp-misc Miscellaneous C++ API * \note Additional example code for this module can be found in its [Tutorial.](@ref controller) */ @@ -22,11 +22,11 @@ #ifndef _PROS_MISC_HPP_ #define _PROS_MISC_HPP_ -#include "pros/misc.h" - #include #include +#include "pros/misc.h" + namespace pros { inline namespace v5 { /** @@ -56,13 +56,13 @@ class Controller { * port. * * \return 1 if the controller is connected, 0 otherwise - * + * * \b Example * \code * void status_display_controller(){ * pros::Controller master(pros::E_CONTROLLER_MASTER); * if(!master.is_connected()) { - * pros::lcd::print(0, "Main controller is not connected!"); + * pros::lcd::print(0, "Main controller is not connected!"); * } * } * \endcode @@ -84,7 +84,7 @@ class Controller { * * \return The current reading of the analog channel: [-127, 127]. * If the controller was not connected, then 0 is returned - * + * * \b Example * \code * void opcontrol() { @@ -107,7 +107,7 @@ class Controller { * port. * * \return The controller's battery capacity - * + * * \b Example * \code * void initialize() { @@ -127,7 +127,7 @@ class Controller { * port. * * \return The controller's battery level - * + * * \b Example * \code * void initialize() { @@ -153,7 +153,7 @@ class Controller { * * \return 1 if the button on the controller is pressed. * If the controller was not connected, then 0 is returned - * + * * \b Example * \code * void opcontrol() { @@ -194,7 +194,7 @@ class Controller { * * \return 1 if the button on the controller is pressed and had not been * pressed the last time this function was called, 0 otherwise. - * + * * \b Example * \code * void opcontrol() { @@ -203,7 +203,7 @@ class Controller { * if (master.get_digital_new_press(pros::E_CONTROLLER_DIGITAL_A)) { * // Toggle pneumatics or other similar actions * } - * + * * delay(2); * } * } @@ -244,7 +244,7 @@ class Controller { * * \return 1 if the operation was successful or PROS_ERR if the operation * failed, setting errno. - * + * * \b Example * \code * void opcontrol() { @@ -286,22 +286,22 @@ class Controller { * * \return 1 if the operation was successful or PROS_ERR if the operation * failed, setting errno. - * + * * \b Example - * \code - * void opcontrol() { - * int count = 0; + * \code + * void opcontrol() { + * int count = 0; * pros::Controller master(pros::E_CONTROLLER_MASTER); - * while (true) { - * if (!(count % 25)) { - * // Only print every 50ms, the controller text update rate is slow - * master.set_text(0, 0, "Example text"); - * } - * count++; - * delay(2); - * } - * } - * \endcode + * while (true) { + * if (!(count % 25)) { + * // Only print every 50ms, the controller text update rate is slow + * master.set_text(0, 0, "Example text"); + * } + * count++; + * delay(2); + * } + * } + * \endcode */ std::int32_t set_text(std::uint8_t line, std::uint8_t col, const char* str); std::int32_t set_text(std::uint8_t line, std::uint8_t col, const std::string& str); @@ -322,16 +322,16 @@ class Controller { * * \return 1 if the operation was successful or PROS_ERR if the operation * failed, setting errno. - * + * * \b Example - * \code - * void opcontrol() { - * pros::Controller master(pros::E_CONTROLLER_MASTER); - * master.set_text(0, 0, "Example"); - * delay(100); - * master.clear_line(0); - * } - * \endcode + * \code + * void opcontrol() { + * pros::Controller master(pros::E_CONTROLLER_MASTER); + * master.set_text(0, 0, "Example"); + * delay(100); + * master.clear_line(0); + * } + * \endcode */ std::int32_t clear_line(std::uint8_t line); @@ -353,7 +353,7 @@ class Controller { * * \return 1 if the operation was successful or PROS_ERR if the operation * failed, setting errno. - * + * * \b Example * \code * void opcontrol() { @@ -386,7 +386,7 @@ class Controller { * * \return 1 if the operation was successful or PROS_ERR if the operation * failed, setting errno. - * + * * \b Example * \code * void opcontrol() { @@ -403,7 +403,7 @@ class Controller { controller_id_e_t _id; ///@} }; -} // namespace v5 +} // namespace v5 namespace battery { /** @@ -418,7 +418,7 @@ namespace battery { * EACCES - Another resource is currently trying to access the battery port. * * \return The current voltage of the battery - * + * * \b Example * \code * void initialize() { @@ -436,7 +436,7 @@ double get_capacity(void); * EACCES - Another resource is currently trying to access the battery port. * * \return The current current of the battery - * + * * \b Example * \code * void initialize() { @@ -454,7 +454,7 @@ int32_t get_current(void); * EACCES - Another resource is currently trying to access the battery port. * * \return The current temperature of the battery - * + * * \b Example * \code * void initialize() { @@ -472,7 +472,7 @@ double get_temperature(void); * EACCES - Another resource is currently trying to access the battery port. * * \return The current capacity of the battery - * + * * \b Example * \code * void initialize() { @@ -490,7 +490,7 @@ namespace competition { * * \return The competition control status as a mask of bits with * COMPETITION_{ENABLED,AUTONOMOUS,CONNECTED}. - * + * * \b Example * \code * void status_display_task(){ @@ -509,6 +509,8 @@ std::uint8_t get_status(void); std::uint8_t is_autonomous(void); std::uint8_t is_connected(void); std::uint8_t is_disabled(void); +std::uint8_t is_field_control(void); +std::uint8_t is_competition_switch(void); } // namespace competition namespace usd { @@ -516,7 +518,7 @@ namespace usd { * Checks if the SD card is installed. * * \return 1 if the SD card is installed, 0 otherwise - * + * * \b Example * \code * void opcontrol() { @@ -525,6 +527,49 @@ namespace usd { * \endcode */ std::int32_t is_installed(void); +/** + * Lists the files in a directory specified by the path + * Puts the list of file names (NOT DIRECTORIES) into the buffer seperated by newlines + * + * This function uses the following values of errno when an error state is + * reached: + * + * EIO - Hard error occured in the low level disk I/O layer + * EINVAL - file or directory is invalid, or length is invalid + * EBUSY - THe physical drinve cannot work + * ENOENT - cannot find the path or file + * EINVAL - the path name format is invalid + * EACCES - Access denied or directory full + * EEXIST - Access denied + * EROFS - SD card is write protected + * ENXIO - drive number is invalid or not a FAT32 drive + * ENOBUFS - drive has no work area + * ENFILE - too many open files + * + * + * + * \note use a path of "\" to list the files in the main directory NOT "/usd/" + * DO NOT PREPEND YOUR PATHS WITH "/usd/" + * + * \return 1 on success or PROS_ERR on failure setting errno + * + * \b Example + * \code + * void opcontrol() { + * char* test = (char*) malloc(128); + * pros::usd::list_files("/", test, 128); + * pros::delay(200); + * printf("%s\n", test); //Prints the file names in the root directory seperated by newlines + * pros::delay(100); + * pros::list_files("/test", test, 128); + * pros::delay(200); + * printf("%s\n", test); //Prints the names of files in the folder named test seperated by newlines + * pros::delay(100); + * } + * \endcode + */ + +std::int32_t list_files(const char* path, char* buffer, std::int32_t len); } // namespace usd } // namespace pros diff --git a/include/pros/motor_group.hpp b/include/pros/motor_group.hpp index f99576e0f..33edd0f5a 100644 --- a/include/pros/motor_group.hpp +++ b/include/pros/motor_group.hpp @@ -16,7 +16,8 @@ * License, v. 2.0. If a copy of the MPL was not distributed with this * file, You can obtain one at http://mozilla.org/MPL/2.0/. * - * \defgroup cpp-motor-group Motor Groups C++ API + * \defgroup cpp-motor-group Motors C++ API + * \note Additional example code for this module can be found in its [Tutorial](@ref motors). */ #ifndef _PROS_MOTOR_GROUP_HPP_ @@ -40,143 +41,112 @@ class MotorGroup : public virtual AbstractMotor { * @{ */ public: - /** * Constructs a new MotorGroup object. - * + * * This function uses the following values of errno when an error state is - * reached: - * ENXIO - The given value is not within the range of V5 ports |1-21|. - * ENODEV - The port cannot be configured as a motor - * + * reached: + * ENXIO - The given value is not within the range of V5 ports |1-21|. + * ENODEV - The port cannot be configured as a motor + * * \param port - * A initializer list of V5 port numbers from 1 to 21, or from -21 to -1 for reversed motors. + * A initializer list of V5 port numbers from 1 to 21, or from -21 to -1 for reversed motors. * A reversed motor will reverse the input or output movement functions and movement related * telemetry in order to produce consistant behavior with non-reversed motors - * - * \param gearset = pros::v5::MotorGears::green + * + * \param gearset = pros::v5::MotorGears::invalid * Optional parameter for the gearset for the motor. - * set to pros::v5::MotorGears::green if not specifed. - * - * \param encoder_units = pros::v5::MotorUnits::degrees + * Does not explicitly set the motor gearset if it is invalid or not specified + * + * \param encoder_units = pros::v5::MotorUnits::invalid * Optional parameter for the encoder units of the motor - * set to pros::v5::MotorUnits::degrees if not specified by the user - * + * Does not explicitly set the motor units if it is invalid or not specified + * * \b Example - * \code - * void opcontrol() { - * MotorGroup first_mg({1, -2}); //Creates a motor on port 1 and a reversed motor on port 2 with - * with both motors using the green gearset and degrees as the encoder units + * \code + * void opcontrol() { + * MotorGroup first_mg({1, -2}); //Creates a motor on port 1 and a reversed motor on port 2 * MotorGroup rotations_mg({4, 5}, pros::v5::MotorGears::blue, pros::v5::MotorUnits::rotations); - * //Creates a motor group on ports 4 and 5 with blue motors using rotaions as the encoder units - * } + * //Creates a motor group on ports 4 and 5 with blue motors using rotaions as the encoder units + * } * \endcode */ - explicit MotorGroup(const std::initializer_list, const pros::v5::MotorGears gearset = pros::v5::MotorGears::green, - const pros::v5::MotorUnits encoder_units = pros::v5::MotorUnits::degrees); + MotorGroup(const std::initializer_list, + const pros::v5::MotorGears gearset = pros::v5::MotorGears::invalid, + const pros::v5::MotorUnits encoder_units = pros::v5::MotorUnits::invalid); /** * Constructs a new MotorGroup object. - * + * * This function uses the following values of errno when an error state is - * reached: - * + * reached: + * * ENXIO - The given value is not within the range of V5 ports |1-21|. - * + * * ENODEV - The port cannot be configured as a motor - * + * * EDOM - The motor group is empty - * + * * \param port - * A initializer list of V5 port numbers from 1 to 21, or from -21 to -1 for reversed motors. + * A initializer list of V5 port numbers from 1 to 21, or from -21 to -1 for reversed motors. * A reversed motor will reverse the input or output movement functions and movement related * telemetry in order to produce consistant behavior with non-reversed motors - * - * \param gearset = pros::v5::MotorGears::green + * + * \param gearset = pros::v5::MotorGears::invalid + * \param gearset = pros::v5::MotorGears::green * Optional parameter for the gearset for the motor. - * set to pros::v5::MotorGears::green if not specifed. - * - * \param encoder_units = pros::v5::MotorUnits::degrees + * Does not explicitly set the motor gearset if it is invalid or not specified + * + * \param encoder_units = pros::v5::MotorUnits::invalid * Optional parameter for the encoder units of the motor - * set to pros::v5::MotorUnits::degrees if not specified by the user - * + * Does not explicitly set the motor units if it is invalid or not specified + * * \b Example - * \code - * void opcontrol() { - * MotorGroup first_mg({1, -2}); //Creates a motor on port 1 and a reversed motor on port 2 with + * \code + * void opcontrol() { + * MotorGroup first_mg({1, -2}); //Creates a motor on port 1 and a reversed motor on port 2 with * with both motors using the green gearset and degrees as the encoder units * MotorGroup rotations_mg({4, 5}, pros::v5::MotorGears::blue, pros::v5::MotorUnits::rotations); - * //Creates a motor group on ports 4 and 5 with blue motors using rotaions as the encoder units - * } + * //Creates a motor group on ports 4 and 5 with blue motors using rotaions as the encoder units + * } * \endcode */ - explicit MotorGroup(const std::vector& ports, const pros::v5::MotorGears gearset = pros::v5::MotorGears::green, - const pros::v5::MotorUnits encoder_units = pros::v5::MotorUnits::degrees); + MotorGroup(const std::vector& ports, const pros::v5::MotorGears gearset = pros::v5::MotorGears::invalid, + const pros::v5::MotorUnits encoder_units = pros::v5::MotorUnits::invalid); - /** + /** * Constructs a new MotorGroup object from an abstract motor. - * + * * This function uses the following values of errno when an error state is - * reached: - * + * reached: + * * ENXIO - The given value is not within the range of V5 ports |1-21|. - * + * * ENODEV - The port cannot be configured as a motor - * + * * EDOM - The motor group is empty - * + * * \param abstract_motor * THe abstract motor to turn into a motor group * Uses abstract_motor.get_port_all() to get the vector of ports - * - * + * + * * \b Example - * \code - * void opcontrol() { - * MotorGroup first_mg({1, -2}); //Creates a motor on port 1 and a reversed motor on port 2 with + * \code + * void opcontrol() { + * MotorGroup first_mg({1, -2}); //Creates a motor on port 1 and a reversed motor on port 2 with * with both motors using the green gearset and degrees as the encoder units * AbstractMotor abs_mtr_group = first_mg; * MotorGroup new_mg = (MotorGroup) abs_mtr_group; - * } + * } * \endcode */ - - MotorGroup(AbstractMotor& abstract_motor); + + MotorGroup(AbstractMotor& motor_group); + /// \name Motor movement functions /// These functions allow programmers to make motors move ///@{ - /** - * Sets the voltage for the motor group from -128 to 127. - * - * This is designed to map easily to the input from the controller's analog - * stick for simple opcontrol use. The actual behavior of the motor is - * analogous to use of pros::Motor::move() - * - * This function uses the following values of errno when an error state is - * reached: - * ENODEV - The port cannot be configured as a motor - * EDOM - the motor group is empty - * - * \param voltage - * The new voltage from -127 to 127 - * - * \return 1 if the operation was successful or PROS_ERR if the operation - * failed, setting errno. - * - * \b Example - * \code - * void opcontrol() { - * pros::MotorGroup MotorGroup ({1,3}, E_MOTOR_GEARSET_18); - * pros::Controller master (E_CONTROLLER_MASTER); - * while (true) { - * mg = master.get_analog(E_CONTROLLER_ANALOG_LEFT_Y); - * pros::delay(2); - * } - * } - * \endcode - */ - std::int32_t operator=(std::int32_t voltage) const; - /** * Sets the voltage for the motor group from -127 to 127. * @@ -354,10 +324,9 @@ class MotorGroup : public virtual AbstractMotor { */ std::int32_t move_voltage(const std::int32_t voltage) const; - /** * Stops the motor group using the currently configured brake mode. - * + * * This function sets motor velocity to zero, which will cause it to act * according to the set brake mode. If brake mode is set to MOTOR_BRAKE_HOLD, * this function may behave differently than calling move_absolute(0) @@ -366,22 +335,22 @@ class MotorGroup : public virtual AbstractMotor { * This function uses the following values of errno when an error state is * reached: * ENODEV - The port cannot be configured as a motor - * + * * EDOM - The motor group is empty - * - * \return 1 if the operation was successful or PROS_ERR if the operation - * failed, setting errno. - * - * \b Example - * \code - * void autonomous() { - * Motor motor(1); - * mg.move_voltage(12000); - * pros::delay(1000); // Move at max voltage for 1 second - * motor.brake(); - * } - * \endcode - */ + * + * \return 1 if the operation was successful or PROS_ERR if the operation + * failed, setting errno. + * + * \b Example + * \code + * void autonomous() { + * Motor motor(1); + * mg.move_voltage(12000); + * pros::delay(1000); // Move at max voltage for 1 second + * motor.brake(); + * } + * \endcode + */ std::int32_t brake(void) const; /** @@ -400,7 +369,7 @@ class MotorGroup : public virtual AbstractMotor { * * \return 1 if the operation was successful or PROS_ERR if the operation * failed, setting errno. - * + * * \b Example * \code * void autonomous() { @@ -412,7 +381,7 @@ class MotorGroup : public virtual AbstractMotor { * \endcode */ std::int32_t modify_profiled_velocity(const std::int32_t velocity) const; - + /** * Gets the target position set for a motor in the motor group, with a parameter * for the motor index. @@ -422,7 +391,7 @@ class MotorGroup : public virtual AbstractMotor { * ENODEV - The port cannot be configured as a motor * EDOM - The motor group is empty * EOVERFLOW - The index is greater than or equal to MotorGroup::size() - * + * * * \param index Optional parameter, 0 by default. * The zero indexed index of the motor in the motor group @@ -441,7 +410,7 @@ class MotorGroup : public virtual AbstractMotor { * } * \endcode */ - double get_target_position(const std::uint8_t index = 0) const; + double get_target_position(const std::uint8_t index) const; /** * Gets a vector of the the target positions set for the motor group @@ -468,16 +437,16 @@ class MotorGroup : public virtual AbstractMotor { /** * Gets the velocity commanded to the motor by the user at the index specified. - * + * * This function uses the following values of errno when an error state is * reached: * ENODEV - The port cannot be configured as a motor * EOVERFLOW - The index is greater than or equal to MotorGroup::size() * EDOM - The motor group was empty - * - * \param index Optional parameter. + * + * \param index Optional parameter. * The zero indexed index of the motor in the motor group - * + * * \return The commanded motor velocity from +-100, +-200, or +-600, or * PROS_ERR if the operation failed, setting errno. * @@ -489,7 +458,7 @@ class MotorGroup : public virtual AbstractMotor { * while (true) { * mg.move_velocity(master.get_analog(E_CONTROLLER_ANALOG_LEFT_Y)); * // get the target velocity from motor at index 1. (port 3) - * std::cout << "Motor Velocity: " << mg.get_target_velocity(1); + * std::cout << "Motor Velocity: " << mg.get_target_velocity(1); * pros::delay(2); * } * } @@ -499,12 +468,12 @@ class MotorGroup : public virtual AbstractMotor { /** * Gets a vector of the velocity commanded to the motor by the user - * + * * This function uses the following values of errno when an error state is * reached: * ENODEV - The port cannot be configured as a motor * EDOM - THe motor group is empty - * + * * \return A vector of the commanded motor velocity from +-100, +-200, or +-600, or * PROS_ERR if the operation failed, setting errno. * @@ -515,7 +484,7 @@ class MotorGroup : public virtual AbstractMotor { * pros::Controller master (E_CONTROLLER_MASTER); * while (true) { * mg.move_velocity(master.get_analog(E_CONTROLLER_ANALOG_LEFT_Y)); - * std::cout << "Motor Velocity: " << mg.get_target_velocity_all(); + * std::cout << "Motor Velocity: " << mg.get_target_velocity_all(); * pros::delay(2); * } * } @@ -535,13 +504,13 @@ class MotorGroup : public virtual AbstractMotor { * This function uses the following values of errno when an error state is * reached: * ENODEV - The port cannot be configured as a motor - * + * * EDOM - THe motor group is empty * * EOVERFLOW - The index is greater than or equal to MotorGroup::size() - * - * \param index Optional parameter. - * The zero indexed index of the motor in the motor group + * + * \param index Optional parameter. + * The zero indexed index of the motor in the motor group * * \return The motor's actual velocity in RPM or PROS_ERR_F if the operation * failed, setting errno. @@ -560,14 +529,14 @@ class MotorGroup : public virtual AbstractMotor { * \endcode */ double get_actual_velocity(const std::uint8_t index = 0) const; - + /** * Gets a vector of the the actual velocity of each motor the motor group. * * This function uses the following values of errno when an error state is * reached: * ENODEV - The port cannot be configured as a motor - * + * * EDOM - THe motor group is empty * * \return A vector of the each motor's actual velocity in RPM or PROS_ERR_F if the operation @@ -594,13 +563,13 @@ class MotorGroup : public virtual AbstractMotor { * This function uses the following values of errno when an error state is * reached: * ENODEV - The port cannot be configured as a motor - * + * * EDOM - The motor group is empty * * EOVERFLOW - The index is greater than or equal to MotorGroup::size() - * - * \param index Optional parameter. - * The zero indexed index of the motor in the motor group + * + * \param index Optional parameter. + * The zero indexed index of the motor in the motor group * * \return The motor's current in mA or PROS_ERR if the operation failed, * setting errno. @@ -626,7 +595,7 @@ class MotorGroup : public virtual AbstractMotor { * This function uses the following values of errno when an error state is * reached: * ENODEV - The port cannot be configured as a motor - * + * * EDOM - The motor group is empty * * @@ -654,14 +623,14 @@ class MotorGroup : public virtual AbstractMotor { * This function uses the following values of errno when an error state is * reached: * ENODEV - The port cannot be configured as a motor - * + * * EDOM - The motor group is empty - * + * * EOVERFLOW - The index is greater than or equal to MotorGroup::size() - * + * * \param index Optional parameter, 0 by default. * The zero indexed index of the motor in the motor group - * + * * \return 1 for moving in the positive direction, -1 for moving in the * negative direction, and PROS_ERR if the operation failed, setting errno. * @@ -688,7 +657,7 @@ class MotorGroup : public virtual AbstractMotor { * ENODEV - The port cannot be configured as a motor * * EDOM - The motor group is empty - * + * * \return 1 for moving in the positive direction, -1 for moving in the * negative direction, and PROS_ERR if the operation failed, setting errno. * @@ -717,12 +686,12 @@ class MotorGroup : public virtual AbstractMotor { * This function uses the following values of errno when an error state is * reached: * ENODEV - The port cannot be configured as a motor - * + * * EDOM - The motor group is empty - * + * * EOVERFLOW - The index is greater than or equal to MotorGroup::size() - * - * + * + * * \param index Optional parameter, 0 by default. * The zero indexed index of the motor in the motor group * @@ -754,7 +723,7 @@ class MotorGroup : public virtual AbstractMotor { * This function uses the following values of errno when an error state is * reached: * ENODEV - The port cannot be configured as a motor - * + * * EDOM - THe motor group is empty * * \return A vector containing each motor's efficiency in percent or PROS_ERR_F if the operation @@ -783,9 +752,9 @@ class MotorGroup : public virtual AbstractMotor { * This function uses the following values of errno when an error state is * reached: * ENODEV - The port cannot be configured as a motor - * + * * EDOM - The motor group is empty - * + * * EOVERFLOW - The index is greater than or equal to MotorGroup::size() * * \param index Optional parameter, 0 by default. @@ -815,9 +784,9 @@ class MotorGroup : public virtual AbstractMotor { * This function uses the following values of errno when an error state is * reached: * ENODEV - The port cannot be configured as a motor - * + * * EDOM - The motor group is empty - * + * * * \return A vector containing the bitfields containing each motor's faults. * @@ -865,7 +834,7 @@ class MotorGroup : public virtual AbstractMotor { * \endcode */ std::uint32_t get_flags(const std::uint8_t index = 0) const; - + /** * Gets a vector of the flags set by each motor in the motor groups's operation. * @@ -901,7 +870,7 @@ class MotorGroup : public virtual AbstractMotor { * ENODEV - The port cannot be configured as a motor * EDOM - The motor group is empty * EOVERFLOW - The index is greater than or equal to MotorGroup::size() - * + * * \param index Optional parameter, 0 by default. * The zero indexed index of the motor in the motor group * @@ -956,7 +925,7 @@ class MotorGroup : public virtual AbstractMotor { * ENODEV - The port cannot be configured as a motor * EDOM - The motor group is empty * EOVERFLOW - The index is greater than or equal to MotorGroup::size() - * + * * \param index Optional parameter, 0 by default. * The zero indexed index of the motor in the motor group * @@ -1010,13 +979,13 @@ class MotorGroup : public virtual AbstractMotor { * ENODEV - The port cannot be configured as a motor * EDOM - The motor group is empty * EOVERFLOW - The index is greater than or equal to MotorGroup::size() - * - * + * + * * \param timestamp * A pointer to a time in milliseconds for which the encoder count * will be returned. If NULL, the timestamp at which the encoder * count was read will not be supplied - * + * * \param index Optional parameter, 0 by default. * The zero indexed index of the motor in the motor group * @@ -1080,7 +1049,7 @@ class MotorGroup : public virtual AbstractMotor { * * \param index Optional parameter, 0 by default. * The zero indexed index of the motor in the motor group - * + * * \return The motor's temperature in degrees Celsius or PROS_ERR_F if the * operation failed, setting errno. * @@ -1134,7 +1103,7 @@ class MotorGroup : public virtual AbstractMotor { * * \param index Optional parameter, 0 by default. * The zero indexed index of the motor in the motor group - * + * * \return The motor's torque in Nm or PROS_ERR_F if the operation failed, * setting errno. * @@ -1185,10 +1154,10 @@ class MotorGroup : public virtual AbstractMotor { * ENODEV - The port cannot be configured as a motor * EDOM - The motor group is empty * EOVERFLOW - The index is greater than or equal to MotorGroup::size() - * + * * \param index Optional parameter, 0 by default. * The zero indexed index of the motor in the motor group - * + * * \return The motor's voltage in mV or PROS_ERR_F if the operation failed, * setting errno. * @@ -1240,10 +1209,10 @@ class MotorGroup : public virtual AbstractMotor { * ENODEV - The port cannot be configured as a motor * EDOM - The motor group is empty * EOVERFLOW - The index is greater than or equal to MotorGroup::size() - * + * * \param index Optional parameter, 0 by default. * The zero indexed index of the motor in the motor group - * + * * \return 1 if the motor's current limit is being exceeded and 0 if the * current limit is not exceeded, or PROS_ERR if the operation failed, setting * errno. @@ -1270,9 +1239,8 @@ class MotorGroup : public virtual AbstractMotor { * ENODEV - The port cannot be configured as a motor * EDOM - The motor group is empty * - * \return A vector containing the following for each motor: 1 if the motor's current limit is being exceeded and 0 if the - * current limit is not exceeded, or PROS_ERR if the operation failed, setting - * errno. + * \return A vector containing the following for each motor: 1 if the motor's current limit is being exceeded and 0 if + * the current limit is not exceeded, or PROS_ERR if the operation failed, setting errno. * * \b Example * \code @@ -1297,7 +1265,7 @@ class MotorGroup : public virtual AbstractMotor { * ENODEV - The port cannot be configured as a motor * EDOM - The motor group is empty * EOVERFLOW - The index is greater than or equal to MotorGroup::size() - * + * * \param index Optional parameter, 0 by default. * The zero indexed index of the motor in the motor group * @@ -1358,11 +1326,11 @@ class MotorGroup : public virtual AbstractMotor { * ENODEV - The port cannot be configured as a motor * EDOM - The motor group is empty * EOVERFLOW - The index is greater than or equal to MotorGroup::size() - * + * * \param index Optional parameter, 0 by default. * The zero indexed index of the motor in the motor group * - * \return One of Motor_Brake, according to what was set for the + * \return One of MotorBrake, according to what was set for the * motor, or E_MOTOR_BRAKE_INVALID if the operation failed, setting errno. * * \b Example @@ -1383,7 +1351,7 @@ class MotorGroup : public virtual AbstractMotor { * ENODEV - The port cannot be configured as a motor * EDOM - The motor group is empty * - * \return A vector with one of Motor_Brake for each motor in the motor group, according to what was set for the + * \return A vector with one of MotorBrake for each motor in the motor group, according to what was set for the * motor, or E_MOTOR_BRAKE_INVALID if the operation failed, setting errno. * * \b Example @@ -1407,7 +1375,7 @@ class MotorGroup : public virtual AbstractMotor { * ENODEV - The port cannot be configured as a motor * EDOM - The motor group is empty * EOVERFLOW - The index is greater than or equal to MotorGroup::size() - * + * * \param index Optional parameter, 0 by default. * The zero indexed index of the motor in the motor group * @@ -1426,7 +1394,7 @@ class MotorGroup : public virtual AbstractMotor { * \endcode */ std::int32_t get_current_limit(const std::uint8_t index = 0) const; - + /** * Gets a vector of the current limit for each motor in the motor group in mA. * @@ -1461,11 +1429,11 @@ class MotorGroup : public virtual AbstractMotor { * ENODEV - The port cannot be configured as a motor * EDOM - The motor group is empty * EOVERFLOW - The index is greater than or equal to MotorGroup::size() - * + * * \param index Optional parameter, 0 by default. * The zero indexed index of the motor in the motor group - * - * \return One of Motor_Units according to what is set for the + * + * \return One of MotorUnits according to what is set for the * motor or E_MOTOR_ENCODER_INVALID if the operation failed. * * \b Example @@ -1477,7 +1445,7 @@ class MotorGroup : public virtual AbstractMotor { * \endcode */ MotorUnits get_encoder_units(const std::uint8_t index = 0) const; - + /** * Gets a vector of the encoder units that were set for each motor in the motor group. * @@ -1486,7 +1454,7 @@ class MotorGroup : public virtual AbstractMotor { * ENODEV - The port cannot be configured as a motor * EDOM - The motor group is empty * - * \return A vector with the following for each motor, One of Motor_Units according to what is set for the + * \return A vector with the following for each motor, One of MotorUnits according to what is set for the * motor or E_MOTOR_ENCODER_INVALID if the operation failed. * * \b Example @@ -1507,12 +1475,12 @@ class MotorGroup : public virtual AbstractMotor { * ENODEV - The port cannot be configured as a motor * EDOM - The motor group is empty * EOVERFLOW - The index is greater than or equal to MotorGroup::size() - * + * *\param index Optional parameter, 0 by default. * The zero indexed index of the motor in the motor group - * - * \return One of Motor_Gears according to what is set for the motor, - * or pros::Motor_Gears::invalid if the operation failed. + * + * \return One of MotorGears according to what is set for the motor, + * or pros::MotorGears::invalid if the operation failed. * * \b Example * \code @@ -1531,9 +1499,9 @@ class MotorGroup : public virtual AbstractMotor { * ENODEV - The port cannot be configured as a motor * EDOM - The motor group is empty * - * - * \return A vector with one of Motor_Gears according to what is set for the motor, - * or pros::Motor_Gears::invalid if the operation failed for each motor. + * + * \return A vector with one of MotorGears according to what is set for the motor, + * or pros::MotorGears::invalid if the operation failed for each motor. * * \b Example * \code @@ -1548,7 +1516,7 @@ class MotorGroup : public virtual AbstractMotor { /** * Gets a vector with all the port numbers in the motor group. * A port will be negative if the motor in the motor group is reversed - * + * * @return a vector with all the port numbers for the motor group */ std::vector get_port_all(void) const; @@ -1564,7 +1532,7 @@ class MotorGroup : public virtual AbstractMotor { * ENODEV - The port cannot be configured as a motor * EDOM - The motor group is empty * EOVERFLOW - The index is greater than or equal to MotorGroup::size() - * + * *\param index Optional parameter, 0 by default. * The zero indexed index of the motor in the motor group * @@ -1582,7 +1550,7 @@ class MotorGroup : public virtual AbstractMotor { std::int32_t get_voltage_limit(const std::uint8_t index = 0) const; /** - * Gets a vector of the voltage limit of each motor in the motor group + * Gets a vector of the voltage limit of each motor in the motor group * * Default value is 0V, which means that there is no software limitation * imposed on the voltage. @@ -1610,10 +1578,10 @@ class MotorGroup : public virtual AbstractMotor { * * This function uses the following values of errno when an error state is * reached: - * + * * EDOM - The motor group is empty * EOVERFLOW - The index is greater than or equal to MotorGroup::size() - * + * *\param index Optional parameter, 0 by default. * The zero indexed index of the motor in the motor group * @@ -1637,8 +1605,8 @@ class MotorGroup : public virtual AbstractMotor { * reached: * EDOM - The motor group is empty * - * \return A vector conatining the following for each motor: 1 if the motor has been reversed and 0 if the motor was not - * reversed, or PROS_ERR if the operation failed, setting errno. + * \return A vector conatining the following for each motor: 1 if the motor has been reversed and 0 if the motor was + * not reversed, or PROS_ERR if the operation failed, setting errno. * * \b Example * \code @@ -1652,7 +1620,7 @@ class MotorGroup : public virtual AbstractMotor { std::vector is_reversed_all(void) const; /** - * Sets one of Motor_Brake to a motor in the motor group. Works with the C enum + * Sets one of MotorBrake to a motor in the motor group. Works with the C enum * and the C++ enum class. * * This function uses the following values of errno when an error state is @@ -1660,13 +1628,13 @@ class MotorGroup : public virtual AbstractMotor { * ENODEV - The port cannot be configured as a motor * EDOM - The motor group is empty * EOVERFLOW - The index is greater than or equal to MotorGroup::size() - * + * * \param mode - * The Motor_Brake to set for the motor - * + * The MotorBrake to set for the motor + * * \param index Optional parameter, 0 by default. * The zero indexed index of the motor in the motor group - * + * * * \return 1 if the operation was successful or PROS_ERR if the operation * failed, setting errno. @@ -1682,7 +1650,7 @@ class MotorGroup : public virtual AbstractMotor { */ std::int32_t set_brake_mode(const MotorBrake mode, const std::uint8_t index = 0) const; /** - * Sets one of Motor_Brake to a motor in the motor group. Works with the C enum + * Sets one of MotorBrake to a motor in the motor group. Works with the C enum * and the C++ enum class. * * This function uses the following values of errno when an error state is @@ -1690,13 +1658,13 @@ class MotorGroup : public virtual AbstractMotor { * ENODEV - The port cannot be configured as a motor * EDOM - The motor group is empty * EOVERFLOW - The index is greater than or equal to MotorGroup::size() - * + * * \param mode - * The Motor_Brake to set for the motor - * + * The MotorBrake to set for the motor + * * \param index Optional parameter, 0 by default. * The zero indexed index of the motor in the motor group - * + * * * \return 1 if the operation was successful or PROS_ERR if the operation * failed, setting errno. @@ -1712,17 +1680,17 @@ class MotorGroup : public virtual AbstractMotor { */ std::int32_t set_brake_mode(const pros::motor_brake_mode_e_t mode, const std::uint8_t index = 0) const; /** - * Sets one of Motor_Brake all the motors in the motor group. Works with the C enum + * Sets one of MotorBrake all the motors in the motor group. Works with the C enum * and the C++ enum class. * * This function uses the following values of errno when an error state is * reached: * ENODEV - The port cannot be configured as a motor * EDOM - The motor group is empty - * + * * \param mode - * The Motor_Brake to set for the motor - * + * The MotorBrake to set for the motor + * * * \return 1 if the operation was successful or PROS_ERR if the operation * failed, setting errno. @@ -1738,18 +1706,18 @@ class MotorGroup : public virtual AbstractMotor { */ std::int32_t set_brake_mode_all(const MotorBrake mode) const; /** - * Sets one of Motor_Brake to a motor in the motor group. Works with the C enum + * Sets one of MotorBrake to a motor in the motor group. Works with the C enum * and the C++ enum class. * * This function uses the following values of errno when an error state is * reached: * ENODEV - The port cannot be configured as a motor * EDOM - The motor group is empty - * + * * \param mode - * The Motor_Brake to set for the motor + * The MotorBrake to set for the motor + * * - * * \return 1 if the operation was successful or PROS_ERR if the operation * failed, setting errno. * @@ -1774,11 +1742,11 @@ class MotorGroup : public virtual AbstractMotor { * * \param limit * The new current limit in mA - * + * * \param index Optional parameter, 0 by default. * The zero indexed index of the motor in the motor group - * - * + * + * * \return 1 if the operation was successful or PROS_ERR if the operation * failed, setting errno. * @@ -1829,20 +1797,20 @@ class MotorGroup : public virtual AbstractMotor { */ std::int32_t set_current_limit_all(const std::int32_t limit) const; /** - * Sets one of Motor_Units for one motor in the motor group's motor encoder. Works with the C + * Sets one of MotorUnits for one motor in the motor group's motor encoder. Works with the C * enum and the C++ enum class. * * This function uses the following values of errno when an error state is * reached: * ENODEV - The port cannot be configured as a motor * EDOM - The motor group is empty - * EOVERFLOW - The index is greater than or equal to MotorGroup::size() + * EOVERFLOW - The index is greater than or equal to MotorGroup::size() * * \param units * The new motor encoder units * \param index Optional parameter, 0 by default. * The zero indexed index of the motor in the motor group - * + * * \return 1 if the operation was successful or PROS_ERR if the operation * failed, setting errno. * @@ -1857,20 +1825,20 @@ class MotorGroup : public virtual AbstractMotor { */ std::int32_t set_encoder_units(const MotorUnits units, const std::uint8_t index = 0) const; /** - * Sets one of Motor_Units for one motor in the motor group's motor encoder. Works with the C + * Sets one of MotorUnits for one motor in the motor group's motor encoder. Works with the C * enum and the C++ enum class. * * This function uses the following values of errno when an error state is * reached: * ENODEV - The port cannot be configured as a motor * EDOM - The motor group is empty - * EOVERFLOW - The index is greater than or equal to MotorGroup::size() + * EOVERFLOW - The index is greater than or equal to MotorGroup::size() * * \param units * The new motor encoder units * \param index Optional parameter, 0 by default. * The zero indexed index of the motor in the motor group - * + * * \return 1 if the operation was successful or PROS_ERR if the operation * failed, setting errno. * @@ -1885,17 +1853,17 @@ class MotorGroup : public virtual AbstractMotor { */ std::int32_t set_encoder_units(const pros::motor_encoder_units_e_t units, const std::uint8_t index = 0) const; /** - * Sets one of Motor_Units for every motor in the motor group's motor encoder. Works with the C + * Sets one of MotorUnits for every motor in the motor group's motor encoder. Works with the C * enum and the C++ enum class. * * This function uses the following values of errno when an error state is * reached: * ENODEV - The port cannot be configured as a motor - * EDOM - The motor group is empty + * EDOM - The motor group is empty * * \param units * The new motor encoder units - * + * * \return 1 if the operation was successful or PROS_ERR if the operation * failed, setting errno. * @@ -1910,17 +1878,17 @@ class MotorGroup : public virtual AbstractMotor { */ std::int32_t set_encoder_units_all(const MotorUnits units) const; /** - * Sets one of Motor_Units for every motor in the motor group's motor encoder. Works with the C + * Sets one of MotorUnits for every motor in the motor group's motor encoder. Works with the C * enum and the C++ enum class. * * This function uses the following values of errno when an error state is * reached: * ENODEV - The port cannot be configured as a motor - * EDOM - The motor group is empty + * EDOM - The motor group is empty * * \param units * The new motor encoder units - * + * * \return 1 if the operation was successful or PROS_ERR if the operation * failed, setting errno. * @@ -1934,6 +1902,7 @@ class MotorGroup : public virtual AbstractMotor { * \endcode */ std::int32_t set_encoder_units_all(const pros::motor_encoder_units_e_t units) const; + /** * Sets one of the gear cartridge (red, green, blue) for one motor in the motor group. Usable with * the C++ enum class and the C enum. @@ -1942,14 +1911,20 @@ class MotorGroup : public virtual AbstractMotor { * reached: * ENODEV - The port cannot be configured as a motor * EDOM - The motor group is empty - * EOVERFLOW - The index is greater than or equal to MotorGroup::size() + * E2BIG - The size of the vector mismatches the number of motors in the motor group + * + * \note If there are more motors than gearsets passed in, + * only the first n motors will have their gearsets changed where n is the number of gearsets passed in. + * If there are more gearsets passed in than motors, then the only the first m gearsets will be used, + * where m is the number of motors. In either case, errno will be set to E2BIG, but the operation still occurs + * * * \param gearset * The new geatset of the motor - * + * * \param index Optional parameter, 0 by default. * The zero indexed index of the motor in the motor group - * + * * \return 1 if the operation was successful or PROS_ERR if the operation * failed, setting errno. * @@ -1962,7 +1937,7 @@ class MotorGroup : public virtual AbstractMotor { * } * \endcode */ - std::int32_t set_gearing(const MotorGears gearset, const std::uint8_t index = 0) const; + std::int32_t set_gearing(std::vector gearsets) const; /** * Sets one of the gear cartridge (red, green, blue) for one motor in the motor group. Usable with * the C++ enum class and the C enum. @@ -1971,14 +1946,14 @@ class MotorGroup : public virtual AbstractMotor { * reached: * ENODEV - The port cannot be configured as a motor * EDOM - The motor group is empty - * EOVERFLOW - The index is greater than or equal to MotorGroup::size() + * EOVERFLOW - The index is greater than or equal to MotorGroup::size() * * \param gearset * The new geatset of the motor - * + * * \param index Optional parameter, 0 by default. * The zero indexed index of the motor in the motor group - * + * * \return 1 if the operation was successful or PROS_ERR if the operation * failed, setting errno. * @@ -1992,6 +1967,68 @@ class MotorGroup : public virtual AbstractMotor { * \endcode */ std::int32_t set_gearing(const pros::motor_gearset_e_t gearset, const std::uint8_t index = 0) const; + + /** + * Sets the gear cartridge (red, green, blue) for each motor in the motor group by taking in a vector of the + * cartridges. Usable with the C++ enum class and the C enum. + * + * This function uses the following values of errno when an error state is + * reached: + * ENODEV - The port cannot be configured as a motor + * EDOM - The motor group is empty + * E2BIG - The size of the vector mismatches the number of motors in the motor group + * + * \note If there are more motors than gearsets passed in, + * only the first n motors will have their gearsets changed where n is the number of gearsets passed in. + * If there are more gearsets passed in than motors, then the only the first m gearsets will be used, + * where m is the number of motors. In either case, errno will be set to E2BIG, but the operation still occurs + * + * \param gearset + * The a vector containing the new geatsets of the motors + * + * \return 1 if the operation was successful or PROS_ERR if the operation + * failed, setting errno. + * + * \b Example + * \code + * void initialize() { + * pros::MotorGroup mg({1,3}); + * mg.set_gearing(pros::MotorGears::blue, 1); + * std::cout << "Gearset: " << mg.get_gearing(); + * } + * \endcode + */ + std::int32_t set_gearing(std::vector gearsets) const; + + /** + * Sets one of the gear cartridge (red, green, blue) for one motor in the motor group. Usable with + * the C++ enum class and the C enum. + * + * This function uses the following values of errno when an error state is + * reached: + * ENODEV - The port cannot be configured as a motor + * EDOM - The motor group is empty + * EOVERFLOW - The index is greater than or equal to MotorGroup::size() + * + * \param gearset + * The new geatset of the motor + * + * \param index Optional parameter, 0 by default. + * The zero indexed index of the motor in the motor group + * + * \return 1 if the operation was successful or PROS_ERR if the operation + * failed, setting errno. + * + * \b Example + * \code + * void initialize() { + * pros::MotorGroup mg({1,3}); + * mg.set_gearing(E_MOTOR_GEARSET_06, 1); + * std::cout << "Gearset: " << mg.get_gearing(); + * } + * \endcode + */ + std::int32_t set_gearing(const MotorGears gearset, const std::uint8_t index = 0) const; /** * Sets one of the gear cartridge (red, green, blue) for one motor in the motor group. Usable with * the C++ enum class and the C enum. @@ -2003,7 +2040,7 @@ class MotorGroup : public virtual AbstractMotor { * * \param gearset * The new geatset of the motor - * + * * \return 1 if the operation was successful or PROS_ERR if the operation * failed, setting errno. * @@ -2028,7 +2065,7 @@ class MotorGroup : public virtual AbstractMotor { * * \param gearset * The new geatset of the motor - * + * * \return 1 if the operation was successful or PROS_ERR if the operation * failed, setting errno. * @@ -2051,8 +2088,8 @@ class MotorGroup : public virtual AbstractMotor { * This function uses the following values of errno when an error state is * reached: * EDOM - The motor group is empty - * EOVERFLOW - The index is greater than or equal to MotorGroup::size() - * + * EOVERFLOW - The index is greater than or equal to MotorGroup::size() + * * \param reverse * True reverses the motor, false is default * @@ -2082,7 +2119,7 @@ class MotorGroup : public virtual AbstractMotor { * This function uses the following values of errno when an error state is * reached: * EDOM - The motor group is empty - * + * * \param reverse * True reverses the motor, false is default * @@ -2107,11 +2144,11 @@ class MotorGroup : public virtual AbstractMotor { * reached: * ENODEV - The port cannot be configured as a motor * EDOM - The motor group is empty - * EOVERFLOW - The index is greater than or equal to MotorGroup::size() + * EOVERFLOW - The index is greater than or equal to MotorGroup::size() * * \param limit * The new voltage limit in Volts - * + * * \param index Optional parameter, 0 by default. * The zero indexed index of the motor in the motor group * @@ -2176,8 +2213,8 @@ class MotorGroup : public virtual AbstractMotor { * reached: * ENODEV - The port cannot be configured as a motor * EDOM - The motor group is empty - * EOVERFLOW - The index is greater than or equal to MotorGroup::size() - * + * EOVERFLOW - The index is greater than or equal to MotorGroup::size() + * * \param position * The new reference position in its encoder units * \param index Optional parameter, 0 by default. @@ -2239,11 +2276,11 @@ class MotorGroup : public virtual AbstractMotor { * reached: * ENODEV - The port cannot be configured as a motor * EDOM - The motor group is empty - * EOVERFLOW - The index is greater than or equal to MotorGroup::size() + * EOVERFLOW - The index is greater than or equal to MotorGroup::size() * * \param index Optional parameter, 0 by default. * The zero indexed index of the motor in the motor group - * + * * \return 1 if the operation was successful or PROS_ERR if the operation * failed, setting errno. * @@ -2255,14 +2292,14 @@ class MotorGroup : public virtual AbstractMotor { * mg.move_absolute(100, 100); // This does not cause a movement * * mg.tare_position(); - * mg.tare_position(1); - * + * mg.tare_position(1); + * * mg.move_absolute(100, 100); // Moves 100 units forward * } * \endcode */ std::int32_t tare_position(const std::uint8_t index = 0) const; - + /** * Sets the "absolute" zero position of every motor in the motor group to its current position. * @@ -2294,37 +2331,37 @@ class MotorGroup : public virtual AbstractMotor { std::int8_t size(void) const; /** - * Gets the port of a motor in the motor group - * - * * \param index Optional parameter, 0 by default. + * Gets the port of a motor in the motor group via index + * + * \param index Optional parameter, 0 by default. * The zero indexed index of the motor in the motor group - * - * \return The port of the motor at the specified index. + * + * \return The port of the motor at the specified index. * The return value is negative if the corresponding motor is reversed - */ + */ std::int8_t get_port(const std::uint8_t index = 0) const; /** * Appends all the motors in the other motor group reference to this motor group - * + * * Maintains the order of the other motor group * */ - void operator+=(MotorGroup&); + void operator+=(AbstractMotor&); /** * Appends all the motors in the other motor group reference to this motor group - * + * * Maintains the order of the other motor group * */ - void append(MotorGroup&); + void append(AbstractMotor&); /** * Removes the all motors on the port (regardless of reversal) from the motor group * * \param port The port to remove from the motor group - * + * */ void erase_port(std::int8_t port); @@ -2332,7 +2369,7 @@ class MotorGroup : public virtual AbstractMotor { private: /** * The ordered vector of ports used by the motor group - */ + */ std::vector _ports; mutable pros::Mutex _MotorGroup_mutex; }; diff --git a/include/pros/motors.h b/include/pros/motors.h index ad6ef34a5..acf61e7ac 100644 --- a/include/pros/motors.h +++ b/include/pros/motors.h @@ -241,7 +241,7 @@ int32_t motor_move_relative(int8_t port, double position, const int32_t velocity int32_t motor_move_velocity(int8_t port, const int32_t velocity); /** - * Sets the output voltage for the motor from -12000 to 12000 in millivolts + * Sets the output voltage for the motor from -12000 to 12000 in millivolts. * * \note A negative port negates the voltage * diff --git a/include/pros/motors.hpp b/include/pros/motors.hpp index f0de7ebc8..b64691163 100644 --- a/include/pros/motors.hpp +++ b/include/pros/motors.hpp @@ -40,102 +40,45 @@ class Motor : public AbstractMotor, public Device { /** * Constructs a new Motor object. - * + * * This function uses the following values of errno when an error state is - * reached: - * ENXIO - The given value is not within the range of V5 ports |1-21|. - * ENODEV - The port cannot be configured as a motor - * + * reached: + * ENXIO - The given value is not within the range of V5 ports |1-21|. + * ENODEV - The port cannot be configured as a motor + * * \param port - * The V5 port number from 1 to 21, or from -21 to -1 for reversed motors. + * The V5 port number from 1 to 21, or from -21 to -1 for reversed motors. * A reversed motor will reverse the input or output movement functions and movement related * telemetry in order to produce consistant behavior with non-reversed motors - * - * \param gearset = pros::v5::MotorGears::green + * + * \param gearset = pros::v5::MotorGears::green * Optional parameter for the gearset for the motor. - * set to pros::v5::MotorGears::green if not specifed. - * + * Does not explicitly set the gearset if not specified or if the gearset is invalid + * * \param encoder_units = pros::v5::MotorUnits::degrees * Optional parameter for the encoder units of the motor - * set to pros::v5::MotorUnits::degrees if not specified by the user - * - * \b Example - * \code - * void opcontrol() { - * Motor first_motor(1); //Creates a motor on port 1 with green gearset and degrees as the encoder units - * Motor reversed_motor(-2); //Creates a reversed motor on port 1 with standard gearset and encoder units - * Motor blue_motor(3, pros::v5::MotorGears::blue); //Creates a motor on port 3 with blue gear set and degrees - * Motor rotations_motor(4, pros::v5::MotorGears::green, pros::v5::MotorUnits::rotations); port 4 w/ rotations - * - * } - * \endcode - * - */ - explicit Motor(const std::int8_t port, const pros::v5::MotorGears gearset = pros::v5::MotorGears::green, - const pros::v5::MotorUnits encoder_units = pros::v5::MotorUnits::degrees); - - - - /** - * Constructs a new Motor object. - * - * This function uses the following values of errno when an error state is - * reached: - * ENXIO - The given value is not within the range of V5 ports |1-21|. - * ENODEV - The port cannot be configured as a motor - * - * \param The abstract motor to create into a motor - * Creates a new motor on the port of abstract_motor.get_port(), maintaining it's reversal status. - * - * - * \b Example - * \code - * void opcontrol() { - * Motor first_motor(1); //Creates a motor on port 1 with green gearset and degrees as the encoder units - * AbstractMotor abs_motor = first_motor; - * Motor new_motor = (Motor) abs_motor; - * - * } - * \endcode - * - */ - Motor(AbstractMotor& abstract_motor); - - - /// \name Motor movement functions - /// These functions allow programmers to make motors move - ///@{ - - /** - * Sets the voltage for the motor from -128 to 127. - * - * This is designed to map easily to the input from the controller's analog - * stick for simple opcontrol use. The actual behavior of the motor is - * analogous to use of pros::Motor::move(). - * - * This function uses the following values of errno when an error state is - * reached: - * ENODEV - The port cannot be configured as a motor - * - * \param voltage - * The new motor voltage from -127 to 127 - * - * \return 1 if the operation was successful or PROS_ERR if the operation - * failed, setting errno. + * Does not explicitly set the gearset if not specified or if the gearset is invalid * * \b Example * \code * void opcontrol() { - * pros::Motor motor (1, E_MOTOR_GEARSET_18); - * pros::Controller master (E_CONTROLLER_MASTER); - * while (true) { - * motor = master.get_analog(E_CONTROLLER_ANALOG_LEFT_Y); - * pros::delay(2); - * } + * Motor first_motor(1); //Creates a motor on port 1 without altering gearset or encoder units + * Motor reversed_motor(-2); //Creates a reversed motor on port 1 port 1 without altering gearset or encoder units + * Motor blue_motor(3, pros::v5::MotorGears::blue); //Creates a motor on port 3 with blue gear set + * Motor rotations_motor(4, pros::v5::MotorGears::green, pros::v5::MotorUnits::rotations); //port 4 w/ rotations + * * } * \endcode + * */ - std::int32_t operator=(std::int32_t voltage) const; + Motor(const std::int8_t port, const pros::v5::MotorGears gearset = pros::v5::MotorGears::invalid, + const pros::v5::MotorUnits encoder_units = pros::v5::MotorUnits::invalid); + + Motor(const Device& device) : Motor(device.get_port()){}; + + /// \name Motor movement functions + /// These functions allow programmers to make motors move + ///@{ /** * Sets the voltage for the motor from -127 to 127. @@ -177,7 +120,7 @@ class Motor : public AbstractMotor, public Device { * * \note This function simply sets the target for the motor, it does not block * program execution until the movement finishes. - * + * * * This function uses the following values of errno when an error state is * reached: @@ -312,10 +255,10 @@ class Motor : public AbstractMotor, public Device { * \endcode */ std::int32_t move_voltage(const std::int32_t voltage) const; - + /** * Stops the motor using the currently configured brake mode. - * + * * This function sets motor velocity to zero, which will cause it to act * according to the set brake mode. If brake mode is set to MOTOR_BRAKE_HOLD, * this function may behave differently than calling move_absolute(0) @@ -324,20 +267,20 @@ class Motor : public AbstractMotor, public Device { * This function uses the following values of errno when an error state is * reached: * ENODEV - The port cannot be configured as a motor - * - * \return 1 if the operation was successful or PROS_ERR if the operation - * failed, setting errno. - * - * \b Example - * \code - * void autonomous() { - * Motor motor(1); - * motor.move_voltage(12000); - * pros::delay(1000); // Move at max voltage for 1 second - * motor.brake(); - * } - * \endcode - */ + * + * \return 1 if the operation was successful or PROS_ERR if the operation + * failed, setting errno. + * + * \b Example + * \code + * void autonomous() { + * Motor motor(1); + * motor.move_voltage(12000); + * pros::delay(1000); // Move at max voltage for 1 second + * motor.brake(); + * } + * \endcode + */ std::int32_t brake(void) const; /** @@ -355,7 +298,7 @@ class Motor : public AbstractMotor, public Device { * * \return 1 if the operation was successful or PROS_ERR if the operation * failed, setting errno. - * + * * \b Example * \code * void autonomous() { @@ -368,19 +311,25 @@ class Motor : public AbstractMotor, public Device { */ std::int32_t modify_profiled_velocity(const std::int32_t velocity) const; + ///@} + + /// \name Motor telemetry functions + /// These functions allow programmers to collect telemetry from motors + ///@{ + /** * Gets the target position set for the motor by the user * - * \note This is one of many Motor functions that takes in an optional index parameter. + * \note This is one of many Motor functions that takes in an optional index parameter. * This parameter can be ignored by most users but exists to give a shared base class * for motors and motor groups - * + * * This function uses the following values of errno when an error state is * reached: * ENODEV - The port cannot be configured as a motor * EOVERFLOW - The index is non 0 - * - * \param index Optional parameter. + * + * \param index Optional parameter. * The zero-indexed index of the motor to get the target position of. * By default index is 0, and will return an error for a non-zero index * @@ -402,19 +351,19 @@ class Motor : public AbstractMotor, public Device { /** * Gets the velocity commanded to the motor by the user at the index specified. * - * \note This is one of many Motor functions that takes in an optional index parameter. + * \note This is one of many Motor functions that takes in an optional index parameter. * This parameter can be ignored by most users but exists to give a shared base class * for motors and motor groups - * + * * This function uses the following values of errno when an error state is * reached: * ENODEV - The port cannot be configured as a motor * EOVERFLOW - The index is non 0 - * - * \param index Optional parameter. + * + * \param index Optional parameter. * The zero-indexed index of the motor to get the target position of. * By default index is 0, and will return an error for a non-zero index - * + * * \return The commanded motor velocity from +-100, +-200, or +-600, or * PROS_ERR if the operation failed, setting errno. * @@ -434,28 +383,22 @@ class Motor : public AbstractMotor, public Device { */ std::int32_t get_target_velocity(const std::uint8_t index = 0) const; - ///@} - - /// \name Motor telemetry functions - /// These functions allow programmers to collect telemetry from motors - ///@{ - /** * Gets the actual velocity of the motor. * - * \note This is one of many Motor functions that takes in an optional index parameter. + * \note This is one of many Motor functions that takes in an optional index parameter. * This parameter can be ignored by most users but exists to give a shared base class * for motors and motor groups - * + * * This function uses the following values of errno when an error state is * reached: * ENODEV - The port cannot be configured as a motor * EOVERFLOW - The index is non 0 - * - * \param index Optional parameter. + * + * \param index Optional parameter. * The zero-indexed index of the motor to get the target position of. - * By default index is 0, and will return an error for a non-zero index - * + * By default index is 0, and will return an error for a non-zero index + * * \return The motor's actual velocity in RPM or PROS_ERR_F if the operation * failed, setting errno. * @@ -475,22 +418,22 @@ class Motor : public AbstractMotor, public Device { /** * Gets the current drawn by the motor in mA. - * - * \note This is one of many Motor functions that takes in an optional index parameter. + * + * \note This is one of many Motor functions that takes in an optional index parameter. * This parameter can be ignored by most users but exists to give a shared base class * for motors and motor groups - * + * * This function uses the following values of errno when an error state is * reached: - * + * * ENODEV - The port cannot be configured as a motor - * + * * EOVERFLOW - The index is non 0 - * - * \param index Optional parameter. + * + * \param index Optional parameter. * The zero-indexed index of the motor to get the target position of. * By default index is 0, and will return an error for a non-zero index - * + * * \return The motor's current in mA or PROS_ERR if the operation failed, * setting errno. * @@ -512,21 +455,21 @@ class Motor : public AbstractMotor, public Device { /** * Gets the direction of movement for the motor. * - * \note This is one of many Motor functions that takes in an optional index parameter. + * \note This is one of many Motor functions that takes in an optional index parameter. * This parameter can be ignored by most users but exists to give a shared base class * for motors and motor groups - * + * * This function uses the following values of errno when an error state is * reached: - * + * * ENODEV - The port cannot be configured as a motor - * + * * EOVERFLOW - The index is non 0 - * - * \param index Optional parameter. + * + * \param index Optional parameter. * The zero-indexed index of the motor to get the target position of. * By default index is 0, and will return an error for a non-zero index - * + * * \return 1 for moving in the positive direction, -1 for moving in the * negative direction, and PROS_ERR if the operation failed, setting errno. * @@ -552,19 +495,19 @@ class Motor : public AbstractMotor, public Device { * drawing no electrical power, and an efficiency of 0% means that the motor * is drawing power but not moving. * - * - * \note This is one of many Motor functions that takes in an optional index parameter. + * + * \note This is one of many Motor functions that takes in an optional index parameter. * This parameter can be ignored by most users but exists to give a shared base class * for motors and motor groups - * + * * This function uses the following values of errno when an error state is * reached: - * + * * ENODEV - The port cannot be configured as a motor - * + * * EOVERFLOW - The index is non 0 - * - * \param index Optional parameter. + * + * \param index Optional parameter. * The zero-indexed index of the motor to get the target position of. * By default index is 0, and will return an error for a non-zero index * @@ -591,18 +534,18 @@ class Motor : public AbstractMotor, public Device { * * Compare this bitfield to the bitmasks in pros::motor_fault_e_t. * - * \note This is one of many Motor functions that takes in an optional index parameter. + * \note This is one of many Motor functions that takes in an optional index parameter. * This parameter can be ignored by most users but exists to give a shared base class * for motors and motor groups - * + * * This function uses the following values of errno when an error state is * reached: - * + * * ENODEV - The port cannot be configured as a motor - * + * * EOVERFLOW - The index is non 0 - * - * \param index Optional parameter. + * + * \param index Optional parameter. * The zero-indexed index of the motor to get the target position of. * By default index is 0, and will return an error for a non-zero index * @@ -622,24 +565,24 @@ class Motor : public AbstractMotor, public Device { * \endcode */ std::uint32_t get_faults(const std::uint8_t index = 0) const; - + /** * Gets the flags set by the motor's operation. * * Compare this bitfield to the bitmasks in pros::motor_flag_e_t. * - * \note This is one of many Motor functions that takes in an optional index parameter. + * \note This is one of many Motor functions that takes in an optional index parameter. * This parameter can be ignored by most users but exists to give a shared base class * for motors and motor groups - * + * * This function uses the following values of errno when an error state is * reached: - * + * * ENODEV - The port cannot be configured as a motor - * + * * EOVERFLOW - The index is non 0 - * - * \param index Optional parameter. + * + * \param index Optional parameter. * The zero-indexed index of the motor to get the target position of. * By default index is 0, and will return an error for a non-zero index * @@ -663,18 +606,18 @@ class Motor : public AbstractMotor, public Device { /** * Gets the absolute position of the motor in its encoder units. * - * \note This is one of many Motor functions that takes in an optional index parameter. + * \note This is one of many Motor functions that takes in an optional index parameter. * This parameter can be ignored by most users but exists to give a shared base class * for motors and motor groups - * + * * This function uses the following values of errno when an error state is * reached: - * + * * ENODEV - The port cannot be configured as a motor - * + * * EOVERFLOW - The index is non 0 - * - * \param index Optional parameter. + * + * \param index Optional parameter. * The zero-indexed index of the motor to get the target position of. * By default index is 0, and will return an error for a non-zero index * @@ -699,21 +642,21 @@ class Motor : public AbstractMotor, public Device { /** * Gets the power drawn by the motor in Watts. * - * \note This is one of many Motor functions that takes in an optional index parameter. + * \note This is one of many Motor functions that takes in an optional index parameter. * This parameter can be ignored by most users but exists to give a shared base class * for motors and motor groups - * + * * This function uses the following values of errno when an error state is * reached: - * + * * ENODEV - The port cannot be configured as a motor - * + * * EOVERFLOW - The index is non 0 - * - * \param index Optional parameter. + * + * \param index Optional parameter. * The zero-indexed index of the motor to get the target position of. * By default index is 0, and will return an error for a non-zero index - * + * * \return The motor's power draw in Watts or PROS_ERR_F if the operation * failed, setting errno. * @@ -735,28 +678,28 @@ class Motor : public AbstractMotor, public Device { /** * Gets the raw encoder count of the motor at a given timestamp. * - * \note This is one of many Motor functions that takes in an optional index parameter. + * \note This is one of many Motor functions that takes in an optional index parameter. * This parameter can be ignored by most users but exists to give a shared base class * for motors and motor groups - * + * * This function uses the following values of errno when an error state is * reached: - * + * * ENODEV - The port cannot be configured as a motor - * + * * EOVERFLOW - The index is non 0 - * - * + * + * * \param timestamp * A pointer to a time in milliseconds for which the encoder count * will be returned. If NULL, the timestamp at which the encoder * count was read will not be supplied - * - * \param index Optional parameter. + * + * \param index Optional parameter. * The zero-indexed index of the motor to get the target position of. * By default index is 0, and will return an error for a non-zero index * - * + * * * \return The raw encoder count at the given timestamp or PROS_ERR if the * operation failed. @@ -779,19 +722,19 @@ class Motor : public AbstractMotor, public Device { /** * Gets the temperature of the motor in degrees Celsius. - - * \note This is one of many Motor functions that takes in an optional index parameter. + + * \note This is one of many Motor functions that takes in an optional index parameter. * This parameter can be ignored by most users but exists to give a shared base class * for motors and motor groups - * + * * This function uses the following values of errno when an error state is * reached: - * + * * ENODEV - The port cannot be configured as a motor - * + * * EOVERFLOW - The index is non 0 - * - * \param index Optional parameter. + * + * \param index Optional parameter. * The zero-indexed index of the motor to get the target position of. * By default index is 0, and will return an error for a non-zero index * @@ -816,18 +759,18 @@ class Motor : public AbstractMotor, public Device { /** * Gets the torque generated by the motor in Newton Meters (Nm). * - * \note This is one of many Motor functions that takes in an optional index parameter. + * \note This is one of many Motor functions that takes in an optional index parameter. * This parameter can be ignored by most users but exists to give a shared base class * for motors and motor groups - * + * * This function uses the following values of errno when an error state is * reached: - * + * * ENODEV - The port cannot be configured as a motor - * + * * EOVERFLOW - The index is non 0 - * - * \param index Optional parameter. + * + * \param index Optional parameter. * The zero-indexed index of the motor to get the target position of. * By default index is 0, and will return an error for a non-zero index * @@ -852,21 +795,21 @@ class Motor : public AbstractMotor, public Device { /** * Gets the voltage delivered to the motor in millivolts. * - * \note This is one of many Motor functions that takes in an optional index parameter. + * \note This is one of many Motor functions that takes in an optional index parameter. * This parameter can be ignored by most users but exists to give a shared base class * for motors and motor groups - * + * * This function uses the following values of errno when an error state is * reached: - * + * * ENODEV - The port cannot be configured as a motor - * + * * EOVERFLOW - The index is non 0 - * - * \param index Optional parameter. + * + * \param index Optional parameter. * The zero-indexed index of the motor to get the target position of. * By default index is 0, and will return an error for a non-zero index - * + * * \return The motor's voltage in mV or PROS_ERR_F if the operation failed, * setting errno. * @@ -888,18 +831,18 @@ class Motor : public AbstractMotor, public Device { /** * Checks if the motor is drawing over its current limit. * - * \note This is one of many Motor functions that takes in an optional index parameter. + * \note This is one of many Motor functions that takes in an optional index parameter. * This parameter can be ignored by most users but exists to give a shared base class * for motors and motor groups - * + * * This function uses the following values of errno when an error state is * reached: - * + * * ENODEV - The port cannot be configured as a motor - * + * * EOVERFLOW - The index is non 0 - * - * \param index Optional parameter. + * + * \param index Optional parameter. * The zero-indexed index of the motor to get the target position of. * By default index is 0, and will return an error for a non-zero index * @@ -925,21 +868,21 @@ class Motor : public AbstractMotor, public Device { /** * Gets the temperature limit flag for the motor. * - * \note This is one of many Motor functions that takes in an optional index parameter. + * \note This is one of many Motor functions that takes in an optional index parameter. * This parameter can be ignored by most users but exists to give a shared base class * for motors and motor groups - * + * * This function uses the following values of errno when an error state is * reached: - * + * * ENODEV - The port cannot be configured as a motor - * + * * EOVERFLOW - The index is non 0 - * - * \param index Optional parameter. + * + * \param index Optional parameter. * The zero-indexed index of the motor to get the target position of. * By default index is 0, and will return an error for a non-zero index - * + * * \return 1 if the temperature limit is exceeded and 0 if the temperature is * below the limit, or PROS_ERR if the operation failed, setting errno. * @@ -967,22 +910,22 @@ class Motor : public AbstractMotor, public Device { /** * Gets the brake mode that was set for the motor. * - * \note This is one of many Motor functions that takes in an optional index parameter. + * \note This is one of many Motor functions that takes in an optional index parameter. * This parameter can be ignored by most users but exists to give a shared base class * for motors and motor groups - * + * * This function uses the following values of errno when an error state is * reached: - * + * * ENODEV - The port cannot be configured as a motor - * + * * EOVERFLOW - The index is non 0 - * - * \param index Optional parameter. + * + * \param index Optional parameter. * The zero-indexed index of the motor to get the target position of. * By default index is 0, and will return an error for a non-zero index * - * \return One of Motor_Brake, according to what was set for the + * \return One of MotorBrake, according to what was set for the * motor, or E_MOTOR_BRAKE_INVALID if the operation failed, setting errno. * * \b Example @@ -999,21 +942,21 @@ class Motor : public AbstractMotor, public Device { /** * Gets the current limit for the motor in mA. * - * \note This is one of many Motor functions that takes in an optional index parameter. + * \note This is one of many Motor functions that takes in an optional index parameter. * This parameter can be ignored by most users but exists to give a shared base class * for motors and motor groups - * + * * This function uses the following values of errno when an error state is * reached: - * + * * ENODEV - The port cannot be configured as a motor - * + * * EOVERFLOW - The index is non 0 - * - * \param index Optional parameter. + * + * \param index Optional parameter. * The zero-indexed index of the motor to get the target position of. * By default index is 0, and will return an error for a non-zero index - * + * * \return The motor's current limit in mA or PROS_ERR if the operation failed, * setting errno. * @@ -1029,26 +972,26 @@ class Motor : public AbstractMotor, public Device { * \endcode */ std::int32_t get_current_limit(const std::uint8_t index = 0) const; - + /** * Gets the encoder units that were set for the motor. * - * \note This is one of many Motor functions that takes in an optional index parameter. + * \note This is one of many Motor functions that takes in an optional index parameter. * This parameter can be ignored by most users but exists to give a shared base class * for motors and motor groups - * + * * This function uses the following values of errno when an error state is * reached: - * + * * ENODEV - The port cannot be configured as a motor - * + * * EOVERFLOW - The index is non 0 - * - * \param index Optional parameter. + * + * \param index Optional parameter. * The zero-indexed index of the motor to get the target position of. * By default index is 0, and will return an error for a non-zero index * - * \return One of Motor_Units according to what is set for the + * \return One of MotorUnits according to what is set for the * motor or E_MOTOR_ENCODER_INVALID if the operation failed. * * \b Example @@ -1064,23 +1007,23 @@ class Motor : public AbstractMotor, public Device { /** * Gets the gearset that was set for the motor. * - * \note This is one of many Motor functions that takes in an optional index parameter. + * \note This is one of many Motor functions that takes in an optional index parameter. * This parameter can be ignored by most users but exists to give a shared base class * for motors and motor groups - * + * * This function uses the following values of errno when an error state is * reached: - * + * * ENODEV - The port cannot be configured as a motor - * + * * EOVERFLOW - The index is non 0 - * - * \param index Optional parameter. + * + * \param index Optional parameter. * The zero-indexed index of the motor to get the target position of. * By default index is 0, and will return an error for a non-zero index * - * \return One of Motor_Gears according to what is set for the motor, - * or pros::Motor_Gears::invalid if the operation failed. + * \return One of MotorGears according to what is set for the motor, + * or pros::MotorGears::invalid if the operation failed. * * \b Example * \code @@ -1098,18 +1041,18 @@ class Motor : public AbstractMotor, public Device { * Default value is 0V, which means that there is no software limitation * imposed on the voltage. * - * \note This is one of many Motor functions that takes in an optional index parameter. + * \note This is one of many Motor functions that takes in an optional index parameter. * This parameter can be ignored by most users but exists to give a shared base class * for motors and motor groups - * + * * This function uses the following values of errno when an error state is * reached: - * + * * ENODEV - The port cannot be configured as a motor - * + * * EOVERFLOW - The index is non 0 - * - * \param index Optional parameter. + * + * \param index Optional parameter. * The zero-indexed index of the motor to get the target position of. * By default index is 0, and will return an error for a non-zero index * @@ -1126,16 +1069,16 @@ class Motor : public AbstractMotor, public Device { /** * Gets whether the motor is reversed or not * - * \note This is one of many Motor functions that takes in an optional index parameter. + * \note This is one of many Motor functions that takes in an optional index parameter. * This parameter can be ignored by most users but exists to give a shared base class * for motors and motor groups - * + * * This function uses the following values of errno when an error state is * reached: - * + * * EOVERFLOW - The index is non 0 - * - * \param index Optional parameter. + * + * \param index Optional parameter. * The zero-indexed index of the motor to get the target position of. * By default index is 0, and will return an error for a non-zero index * @@ -1155,22 +1098,22 @@ class Motor : public AbstractMotor, public Device { /** * Sets one of Motor_Brake to the motor. - * \note This is one of many Motor functions that takes in an optional index parameter. + * \note This is one of many Motor functions that takes in an optional index parameter. * This parameter can be ignored by most users but exists to give a shared base class * for motors and motor groups - * + * * This function uses the following values of errno when an error state is * reached: - * + * * ENODEV - The port cannot be configured as a motor - * + * * EOVERFLOW - The index is non 0 - * - * + * + * * \param mode - * The Motor_Brake to set for the motor - * - * \param index Optional parameter. + * The MotorBrake to set for the motor + * + * \param index Optional parameter. * The zero-indexed index of the motor to get the target position of. * By default index is 0, and will return an error for a non-zero index * @@ -1188,23 +1131,23 @@ class Motor : public AbstractMotor, public Device { */ std::int32_t set_brake_mode(const MotorBrake mode, const std::uint8_t index = 0) const; /** - * Sets one of Motor_Brake to the motor. - * \note This is one of many Motor functions that takes in an optional index parameter. + * Sets one of MotorBrake to the motor. + * \note This is one of many Motor functions that takes in an optional index parameter. * This parameter can be ignored by most users but exists to give a shared base class * for motors and motor groups - * + * * This function uses the following values of errno when an error state is * reached: - * + * * ENODEV - The port cannot be configured as a motor - * + * * EOVERFLOW - The index is non 0 - * - * + * + * * \param mode - * The Motor_Brake to set for the motor - * - * \param index Optional parameter. + * The MotorBrake to set for the motor + * + * \param index Optional parameter. * The zero-indexed index of the motor to get the target position of. * By default index is 0, and will return an error for a non-zero index * @@ -1221,25 +1164,24 @@ class Motor : public AbstractMotor, public Device { * \endcode */ std::int32_t set_brake_mode(const pros::motor_brake_mode_e_t mode, const std::uint8_t index = 0) const; - /** * Sets the current limit for the motor in mA. * - * \note This is one of many Motor functions that takes in an optional index parameter. + * \note This is one of many Motor functions that takes in an optional index parameter. * This parameter can be ignored by most users but exists to give a shared base class * for motors and motor groups - * + * * This function uses the following values of errno when an error state is * reached: - * + * * ENODEV - The port cannot be configured as a motor - * + * * EOVERFLOW - The index is non 0 - * - * * \param limit + * + * \param limit * The new current limit in mA - * - * \param index Optional parameter. + * + * \param index Optional parameter. * The zero-indexed index of the motor to get the target position of. * By default index is 0, and will return an error for a non-zero index * @@ -1264,7 +1206,7 @@ class Motor : public AbstractMotor, public Device { std::int32_t set_current_limit(const std::int32_t limit, const std::uint8_t index = 0) const; /** - * Sets one of Motor_Units for the motor encoder. Works with the C + * Sets one of MotorUnits for the motor encoder. Works with the C * enum and the C++ enum class. * * This function uses the following values of errno when an error state is @@ -1288,24 +1230,24 @@ class Motor : public AbstractMotor, public Device { */ std::int32_t set_encoder_units(const MotorUnits units, const std::uint8_t index = 0) const; /** - * Sets one of Motor_Units for the motor encoder. Works with the C + * Sets one of MotorUnits for the motor encoder. Works with the C * enum and the C++ enum class. * - * \note This is one of many Motor functions that takes in an optional index parameter. + * \note This is one of many Motor functions that takes in an optional index parameter. * This parameter can be ignored by most users but exists to give a shared base class * for motors and motor groups - * + * * This function uses the following values of errno when an error state is * reached: - * + * * ENODEV - The port cannot be configured as a motor - * + * * EOVERFLOW - The index is non 0 - * - * \param index Optional parameter. + * + * \param index Optional parameter. * The zero-indexed index of the motor to get the target position of. * By default index is 0, and will return an error for a non-zero index - * + * * * \param units * The new motor encoder units * @@ -1322,23 +1264,22 @@ class Motor : public AbstractMotor, public Device { * \endcode */ std::int32_t set_encoder_units(const pros::motor_encoder_units_e_t units, const std::uint8_t index = 0) const; - /** * Sets one of the gear cartridge (red, green, blue) for the motor. Usable with * the C++ enum class and the C enum. * - * \note This is one of many Motor functions that takes in an optional index parameter. + * \note This is one of many Motor functions that takes in an optional index parameter. * This parameter can be ignored by most users but exists to give a shared base class * for motors and motor groups - * + * * This function uses the following values of errno when an error state is * reached: - * + * * ENODEV - The port cannot be configured as a motor - * + * * EOVERFLOW - The index is non 0 - * - * \param index Optional parameter. + * + * \param index Optional parameter. * The zero-indexed index of the motor to get the target position of. * By default index is 0, and will return an error for a non-zero index * \param gearset @@ -1357,26 +1298,26 @@ class Motor : public AbstractMotor, public Device { * \endcode */ std::int32_t set_gearing(const MotorGears gearset, const std::uint8_t index = 0) const; - + /** * Sets one of the gear cartridge (red, green, blue) for the motor. Usable with * the C++ enum class and the C enum. - - * \note This is one of many Motor functions that takes in an optional index parameter. + + * \note This is one of many Motor functions that takes in an optional index parameter. * This parameter can be ignored by most users but exists to give a shared base class * for motors and motor groups - * + * * This function uses the following values of errno when an error state is * reached: - * + * * ENODEV - The port cannot be configured as a motor - * + * * EOVERFLOW - The index is non 0 - * + * * \param gearset * The new motor gearset - * - * \param index Optional parameter. + * + * \param index Optional parameter. * The zero-indexed index of the motor to get the target position of. * By default index is 0, and will return an error for a non-zero index * @@ -1400,19 +1341,19 @@ class Motor : public AbstractMotor, public Device { * * This will invert its movements and the values returned for its position. * - * \note This is one of many Motor functions that takes in an optional index parameter. + * \note This is one of many Motor functions that takes in an optional index parameter. * This parameter can be ignored by most users but exists to give a shared base class * for motors and motor groups - * + * * This function uses the following values of errno when an error state is * reached: - * + * * EOVERFLOW - The index is non 0 - * + * * \param reverse * True reverses the motor, false is default direction - * - * \param index Optional parameter. + * + * \param index Optional parameter. * The zero-indexed index of the motor to get the target position of. * By default index is 0, and will return an error for a non-zero index * @@ -1466,25 +1407,25 @@ class Motor : public AbstractMotor, public Device { * This will be the future reference point for the motor's "absolute" * position. * - * \note This is one of many Motor functions that takes in an optional index parameter. + * \note This is one of many Motor functions that takes in an optional index parameter. * This parameter can be ignored by most users but exists to give a shared base class * for motors and motor groups - * + * * This function uses the following values of errno when an error state is * reached: - * + * * ENODEV - The port cannot be configured as a motor - * + * * EOVERFLOW - The index is non 0 - * + * * \param position * The new reference position in its encoder units - * - * \param index Optional parameter. + * + * \param index Optional parameter. * The zero-indexed index of the motor to get the target position of. * By default index is 0, and will return an error for a non-zero index - * - * + * + * * \return 1 if the operation was successful or PROS_ERR if the operation * failed, setting errno. * @@ -1506,21 +1447,21 @@ class Motor : public AbstractMotor, public Device { /** * Sets the "absolute" zero position of the motor to its current position. * - * \note This is one of many Motor functions that takes in an optional index parameter. + * \note This is one of many Motor functions that takes in an optional index parameter. * This parameter can be ignored by most users but exists to give a shared base class * for motors and motor groups - * + * * This function uses the following values of errno when an error state is * reached: - * + * * ENODEV - The port cannot be configured as a motor - * + * * EOVERFLOW - The index is non 0 - * - * \param index Optional parameter. + * + * \param index Optional parameter. * The zero-indexed index of the motor to get the target position of. * By default index is 0, and will return an error for a non-zero index - * + * * \return 1 if the operation was successful or PROS_ERR if the operation * failed, setting errno. * @@ -1540,18 +1481,32 @@ class Motor : public AbstractMotor, public Device { /** * Gets the number of motors. - * + * * \return Always returns 1 - * + * */ std::int8_t size(void) const; + /** + * Gets all motors. + * + * \return A vector of Motor objects. + * + * \b Example + * \code + * void opcontrol() { + * std::vector motor_all = pros::Motor::get_all_devices(); // All motors that are connected + * } + * \endcode + */ + static std::vector get_all_devices(); + /** * gets the port number of the motor * - * \return The signed port of the motor. (negative if the motor is reversed) - * - */ + * \return The signed port of the motor. (negative if the motor is reversed) + * + */ std::int8_t get_port(const std::uint8_t index = 0) const; ///@} @@ -1589,7 +1544,7 @@ class Motor : public AbstractMotor, public Device { * reached: * ENODEV - The port cannot be configured as a motor * - * \return A vector containing the commanded motor velocity from +-100, + * \return A vector containing the commanded motor velocity from +-100, * +-200, or +-600, or PROS_ERR if the operation failed, setting errno. * * \b Example @@ -1631,18 +1586,18 @@ class Motor : public AbstractMotor, public Device { * } * } * \endcode - */ + */ std::vector get_actual_velocity_all(void) const; /** * Gets a vector containing the current drawn by the motor in mA. - * + * * This function uses the following values of errno when an error state is * reached: - * + * * ENODEV - The port cannot be configured as a motor - * - * + * + * * \return A vector containing the motor's current in mA or PROS_ERR if the operation failed, * setting errno. * @@ -1664,13 +1619,13 @@ class Motor : public AbstractMotor, public Device { /** * Gets a vector containing the direction of movement for the motor. * - * + * * This function uses the following values of errno when an error state is * reached: - * + * * ENODEV - The port cannot be configured as a motor - * - * + * + * * \return A vector containing 1 for moving in the positive direction, -1 for moving in the * negative direction, and PROS_ERR if the operation failed, setting errno. * @@ -1695,12 +1650,12 @@ class Motor : public AbstractMotor, public Device { * An efficiency of 100% means that the motor is moving electrically while * drawing no electrical power, and an efficiency of 0% means that the motor * is drawing power but not moving. - * + * * This function uses the following values of errno when an error state is * reached: - * + * * ENODEV - The port cannot be configured as a motor - * + * * * \return A vector containing The motor's efficiency in percent or PROS_ERR_F if the operation * failed, setting errno. @@ -1719,15 +1674,15 @@ class Motor : public AbstractMotor, public Device { * \endcode */ std::vector get_efficiency_all(void) const; - + /** * Gets a vector of the faults experienced by the motor. * * Compare this bitfield to the bitmasks in pros::motor_fault_e_t. - * + * * This function uses the following values of errno when an error state is * reached: - * + * * ENODEV - The port cannot be configured as a motor * * \return A bitfield containing the motor's faults. @@ -1751,10 +1706,10 @@ class Motor : public AbstractMotor, public Device { * Gets a vector of the flags set by the motor's operation. * * Compare this bitfield to the bitmasks in pros::motor_flag_e_t. - * + * * This function uses the following values of errno when an error state is * reached: - * + * * ENODEV - The port cannot be configured as a motor * * @@ -1774,15 +1729,15 @@ class Motor : public AbstractMotor, public Device { * \endcode */ std::vector get_flags_all(void) const; - + /** * Gets a vector containing the absolute position of the motor in its encoder units. - * + * * This function uses the following values of errno when an error state is * reached: - * + * * ENODEV - The port cannot be configured as a motor - + * * \return A vector containing the motor's absolute position in its encoder units or PROS_ERR_F * if the operation failed, setting errno. @@ -1804,12 +1759,12 @@ class Motor : public AbstractMotor, public Device { /** * Gets a vector containing the power drawn by the motor in Watts. - * + * * This function uses the following values of errno when an error state is * reached: - * + * * ENODEV - The port cannot be configured as a motor - * + * * \return A vector containing the motor's power draw in Watts or PROS_ERR_F if the operation * failed, setting errno. * @@ -1827,21 +1782,21 @@ class Motor : public AbstractMotor, public Device { * \endcode */ std::vector get_power_all(void) const; - + /** * Gets a vector of the raw encoder count of the motor at a given timestamp. * - * + * * This function uses the following values of errno when an error state is * reached: - * + * * ENODEV - The port cannot be configured as a motor - * + * * \param timestamp * A pointer to a time in milliseconds for which the encoder count * will be returned. If NULL, the timestamp at which the encoder * count was read will not be supplied - * + * * \return A vector containing the raw encoder count at the given timestamp or PROS_ERR if the * operation failed. * @@ -1860,7 +1815,7 @@ class Motor : public AbstractMotor, public Device { * \endcode */ std::vector get_raw_position_all(std::uint32_t* const timestamp) const; - + /** * Gets a vector of the temperature of the motor in degrees Celsius. * @@ -1868,7 +1823,7 @@ class Motor : public AbstractMotor, public Device { * reached: * ENODEV - The port cannot be configured as a motor * - * \return A vector contaioning the motor's temperature in degrees Celsius + * \return A vector contaioning the motor's temperature in degrees Celsius * or PROS_ERR_F if the operation failed, setting errno. * * \b Example @@ -1892,7 +1847,7 @@ class Motor : public AbstractMotor, public Device { * This function uses the following values of errno when an error state is * reached: * ENODEV - The port cannot be configured as a motor - * + * * \return A vector containing the motor's torque in Nm or PROS_ERR_F if the operation failed, * setting errno. * @@ -1910,14 +1865,14 @@ class Motor : public AbstractMotor, public Device { * \endcode */ std::vector get_torque_all(void) const; - + /** * Gets a vector of the voltage delivered to the motor in millivolts. * * This function uses the following values of errno when an error state is * reached: * ENODEV - The port cannot be configured as a motor - * + * * * \return A vector of the motor's voltage in mV or PROS_ERR_F if the operation failed, * setting errno. @@ -1962,14 +1917,14 @@ class Motor : public AbstractMotor, public Device { * \endcode */ std::vector is_over_current_all(void) const; - + /** * Gets the temperature limit flag for the motor. * * This function uses the following values of errno when an error state is * reached: - * ENODEV - The port cannot be configured as a motor - * + * ENODEV - The port cannot be configured as a motor + * * \return A vector containing 1 if the temperature limit is exceeded and 0 if the temperature is * below the limit, or PROS_ERR if the operation failed, setting errno. * @@ -1987,7 +1942,7 @@ class Motor : public AbstractMotor, public Device { * \endcode */ std::vector is_over_temp_all(void) const; - + /** * Gets a vector containing the brake mode that was set for the motor. * @@ -2008,7 +1963,7 @@ class Motor : public AbstractMotor, public Device { * \endcode */ std::vector get_brake_mode_all(void) const; - + /** * Gets a vector containing the current limit for the motor in mA. * @@ -2053,7 +2008,7 @@ class Motor : public AbstractMotor, public Device { * \endcode */ std::vector get_encoder_units_all(void) const; - + /** * Gets a vector containing the gearset that was set for the motor. * @@ -2077,10 +2032,10 @@ class Motor : public AbstractMotor, public Device { /** * Gets returns a vector with all the port numbers in the motor group. * - * \return A vector containing the signed port of the motor. (negative if the motor is reversed) + * \return A vector containing the signed port of the motor. (negative if the motor is reversed) */ std::vector get_port_all(void) const; - + /** * Gets a vector of the voltage limit set by the user. * @@ -2120,9 +2075,9 @@ class Motor : public AbstractMotor, public Device { * \endcode */ std::vector is_reversed_all(void) const; - + /** - * Sets one of Motor_Brake to the motor. + * Sets one of Motor_Brake to the motor. * * This function uses the following values of errno when an error state is * reached: @@ -2144,9 +2099,9 @@ class Motor : public AbstractMotor, public Device { * \endcode */ std::int32_t set_brake_mode_all(const MotorBrake mode) const; - + /** - * Sets one of Motor_Brake to the motor. + * Sets one of Motor_Brake to the motor. * * This function uses the following values of errno when an error state is * reached: @@ -2203,21 +2158,21 @@ class Motor : public AbstractMotor, public Device { * Sets one of Motor_Units for the motor encoder. Works with the C * enum and the C++ enum class. * - * \note This is one of many Motor functions that takes in an optional index parameter. + * \note This is one of many Motor functions that takes in an optional index parameter. * This parameter can be ignored by most users but exists to give a shared base class * for motors and motor groups - * + * * This function uses the following values of errno when an error state is * reached: - * + * * ENODEV - The port cannot be configured as a motor - * + * * EOVERFLOW - The index is non 0 - * + * * * \param units * The new motor encoder units - * - * \param index Optional parameter. + * + * \param index Optional parameter. * The zero-indexed index of the motor to get the target position of. * By default index is 0, and will return an error for a non-zero index * @@ -2234,7 +2189,7 @@ class Motor : public AbstractMotor, public Device { * \endcode */ std::int32_t set_encoder_units_all(const MotorUnits units) const; - + /** * Sets one of Motor_Units for the motor encoder. Works with the C * enum and the C++ enum class. @@ -2259,8 +2214,7 @@ class Motor : public AbstractMotor, public Device { * \endcode */ std::int32_t set_encoder_units_all(const pros::motor_encoder_units_e_t units) const; - - + /** * Sets one of the gear cartridge (red, green, blue) for the motor. Usable with * the C++ enum class and the C enum. @@ -2285,7 +2239,7 @@ class Motor : public AbstractMotor, public Device { * \endcode */ std::int32_t set_gearing_all(const MotorGears gearset) const; - + /** * Sets one of the gear cartridge (red, green, blue) for the motor. Usable with * the C++ enum class and the C enum. @@ -2310,7 +2264,7 @@ class Motor : public AbstractMotor, public Device { * \endcode */ std::int32_t set_gearing_all(const pros::motor_gearset_e_t gearset) const; - + /** * Sets the reverse flag for the motor. * @@ -2320,7 +2274,7 @@ class Motor : public AbstractMotor, public Device { * \param reverse * True reverses the motor, false is default direction * - * \return 1 + * \return 1 * * \b Example * \code @@ -2332,25 +2286,25 @@ class Motor : public AbstractMotor, public Device { * \endcode */ std::int32_t set_reversed_all(const bool reverse); - + /** * Sets the voltage limit for the motor in Volts. * - * \note This is one of many Motor functions that takes in an optional index parameter. + * \note This is one of many Motor functions that takes in an optional index parameter. * This parameter can be ignored by most users but exists to give a shared base class * for motors and motor groups - * + * * This function uses the following values of errno when an error state is * reached: - * + * * ENODEV - The port cannot be configured as a motor - * + * * EOVERFLOW - The index is non 0 - * + * * \param limit * The new voltage limit in Volts - * - * \param index Optional parameter. + * + * \param index Optional parameter. * The zero-indexed index of the motor to get the target position of. * By default index is 0, and will return an error for a non-zero index * @@ -2373,7 +2327,7 @@ class Motor : public AbstractMotor, public Device { * \endcode */ std::int32_t set_voltage_limit_all(const std::int32_t limit) const; - + /** * Sets the position for the motor in its encoder units. * @@ -2432,10 +2386,39 @@ class Motor : public AbstractMotor, public Device { ///@} private: + /** + * The port of the motor. Negative ports indicate that the motor is reversed + */ std::int8_t _port; }; namespace literals { +/** + * Constructs a Motor from a literal ending in _mtr + * + * \return a pros::Motor for the corresponding port + * + * \b Example + * \code + * using namespace pros::literals; + * void opcontrol() { + * pros::Motor motor = 2_mtr; //Makes an Motor object on port 2 + * } + * \endcode + */ const pros::Motor operator"" _mtr(const unsigned long long int m); +/** + * Constructs a reversed Motor from a literal ending in _rmtr + * + * \return a pros::Motor for the corresponding port that is reversed + * + * \b Example + * \code + * using namespace pros::literals; + * void opcontrol() { + * pros::motor motor = 2_rmtr; //Makes an reversed Motor object on port 2 + * } + * \endcode + */ const pros::Motor operator"" _rmtr(const unsigned long long int m); } // namespace literals } // namespace v5 diff --git a/include/pros/optical.hpp b/include/pros/optical.hpp index 3f700d41c..463daa8a8 100644 --- a/include/pros/optical.hpp +++ b/include/pros/optical.hpp @@ -12,7 +12,7 @@ * This Source Code Form is subject to the terms of the Mozilla Public * License, v. 2.0. If a copy of the MPL was not distributed with this * file, You can obtain one at http://mozilla.org/MPL/2.0/. - * + * * \defgroup cpp-optical VEX Optical Sensor C++ API */ @@ -24,8 +24,8 @@ #include #include -#include "pros/optical.h" #include "pros/device.hpp" +#include "pros/optical.h" namespace pros { inline namespace v5 { @@ -48,13 +48,30 @@ class Optical : public Device { * * \param port * The V5 port number from 1-21 - * - * \b Example: + * + * \b Example: * \code{.cpp} * pros::Optical optical(1); * \endcode */ - explicit Optical(const std::uint8_t port); + Optical(const std::uint8_t port); + + Optical(const Device& device) : Optical(device.get_port()){}; + + + /** + * Gets all optical sensors. + * + * \return A vector of Optical sensor objects. + * + * \b Example + * \code + * void opcontrol() { + * std::vector optical_all = pros::Optical::get_all_devices(); // All optical sensors that are connected + * } + * \endcode + */ + static std::vector get_all_devices(); /** * Get the detected color hue @@ -93,7 +110,7 @@ class Optical : public Device { * * \return saturation value if the operation was successful or PROS_ERR_F if * the operation failed, setting errno. - * + * * \b Example: * \code{.cpp} * void opcontrol() { @@ -124,6 +141,7 @@ class Optical : public Device { * pros::Optical optical(1); * std::cout << "Brightness: " << optical.get_brightness() << std::endl; * } + * \endcode */ virtual double get_brightness(); @@ -161,8 +179,8 @@ class Optical : public Device { * ENXIO - The given value is not within the range of V5 ports (1-21). * ENODEV - The port cannot be configured as an Optical Sensor * - * \return The Error code encountered or PROS_SUCCESS. - * + * \return The Error code encountered or PROS_SUCCESS. + * * \b Example: * \code{.cpp} * void initialize() { @@ -205,9 +223,9 @@ class Optical : public Device { * ENXIO - The given value is not within the range of V5 ports (1-21). * ENODEV - The port cannot be configured as an Optical Sensor * - * \return rgb value if the operation was successful or an optical_rgb_s_t + * \return rgb value if the operation was successful or an optical_rgb_s_t * with all fields set to PROS_ERR if the operation failed, setting errno. - * + * * \b Example: * \code{.cpp} * void opcontrol() { @@ -233,8 +251,23 @@ class Optical : public Device { * ENXIO - The given value is not within the range of V5 ports (1-21). * ENODEV - The port cannot be configured as an Optical Sensor * - * \return raw rgb value if the operation was successful or an optical_raw_s_t + * \return raw rgb value if the operation was successful or an optical_raw_s_t * with all fields set to PROS_ERR if the operation failed, setting errno. + * + * \b Example: + * \code{.cpp} + * void opcontrol() { + * pros::Optical optical(1); + * pros::c::optical_raw_s_t raw = optical.get_raw(); + * while (1) { + * std::cout << "Red: " << raw.red << std::endl; + * std::cout << "Green: " << raw.green << std::endl; + * std::cout << "Blue: " << raw.blue << std::endl; + * std::cout << "Clear: " << raw.clear << std::endl; + * pros::delay(20); + * } + * } + * \endcode */ virtual pros::c::optical_raw_s_t get_raw(); @@ -242,10 +275,12 @@ class Optical : public Device { * Get the most recent gesture data from the sensor * * Gestures will be cleared after 500mS - * 0 = no gesture - * 1 = up (towards cable) - * 2 = down - * 3 = right + * + * + * 0 = no gesture, + * 1 = up (towards cable), + * 2 = down, + * 3 = right, * 4 = left * * This function uses the following values of errno when an error state is @@ -277,7 +312,7 @@ class Optical : public Device { * ENXIO - The given value is not within the range of V5 ports (1-21). * ENODEV - The port cannot be configured as an Optical Sensor * - * \return gesture value if the operation was successful or an optical_gesture_s_t + * \return gesture value if the operation was successful or an optical_gesture_s_t * with all fields set to PROS_ERR if the operation failed, setting errno. * * \b Example: @@ -311,8 +346,8 @@ class Optical : public Device { * ENODEV - The port cannot be configured as an Optical Sensor * * \return 1 if the operation is successful or PROS_ERR if the operation failed, - * setting errno. - * + * setting errno. + * * \b Example: * \code{.cpp} * void opcontrol() { @@ -331,6 +366,7 @@ class Optical : public Device { * pros::delay(20); * } * } + * \endcode */ virtual std::int32_t enable_gesture(); @@ -343,7 +379,7 @@ class Optical : public Device { * ENODEV - The port cannot be configured as an Optical Sensor * * \return 1 if the operation is successful or PROS_ERR if the operation failed, - * setting errno. + * setting errno. * * \b Example: * \code{.cpp} @@ -362,12 +398,11 @@ class Optical : public Device { */ virtual std::int32_t disable_gesture(); - /** - * This is the overload for the << operator for printing to streams - * - * Prints in format(this below is all in one line with no new line): - * Optical [port: (port number), hue: (hue), saturation: (saturation), + * This is the overload for the << operator for printing to streams + * + * Prints in format(this below is all in one line with no new line): + * Optical [port: (port number), hue: (hue), saturation: (saturation), * brightness: (brightness), proximity: (proximity), rgb: {red, green, blue}] * * \b Example: @@ -377,15 +412,28 @@ class Optical : public Device { * \endcode */ friend std::ostream& operator<<(std::ostream& os, pros::Optical& optical); - + private: ///@} }; namespace literals { +/** + * Constructs a Optical sensor from a literal ending in _opt + * + * \return a pros::Optical for the corresponding port + * + * \b Example + * \code + * using namespace pros::literals; + * void opcontrol() { + * pros::Optical opt = 2_opt; //Makes an Optical object on port 2 + * } + * \endcode + */ const pros::Optical operator"" _opt(const unsigned long long int o); } // namespace literals -} +} // namespace v5 } // namespace pros #endif diff --git a/include/pros/rotation.hpp b/include/pros/rotation.hpp index 25806c3f3..e7743aa41 100644 --- a/include/pros/rotation.hpp +++ b/include/pros/rotation.hpp @@ -12,7 +12,7 @@ * This Source Code Form is subject to the terms of the Mozilla Public * License, v. 2.0. If a copy of the MPL was not distributed with this * file, You can obtain one at http://mozilla.org/MPL/2.0/. - * + * * \defgroup cpp-rotation VEX Rotation Sensor C++ API */ #ifndef _PROS_ROTATION_HPP_ @@ -21,8 +21,8 @@ #include #include -#include "pros/rotation.h" #include "pros/device.hpp" +#include "pros/rotation.h" namespace pros { inline namespace v5 { @@ -38,22 +38,25 @@ class Rotation : public Device { public: /** * Constructs a new Rotation Sensor object - * + * * ENXIO - The given value is not within the range of V5 ports |1-21|. - * ENODEV - The port cannot be configured as a Rotation Sensor - * + * ENODEV - The port cannot be configured as a Rotation Sensor + * * \param port - * The V5 port number from 1 to 21, or from -21 to -1 for reversed Rotation Sensors. - * + * The V5 port number from 1 to 21, or from -21 to -1 for reversed Rotation Sensors. + * * \b Example - * \code - * void opcontrol() { + * \code + * void opcontrol() { * pros::Rotation rotation_sensor(1); //Creates a Rotation Sensor on port 1 * pros::Rotation reversed_rotation_sensor(-2); //Creates a reversed Rotation Sensor on port 2 - * } - * \endcode - */ - explicit Rotation(const std::int8_t port); + * } + * \endcode + */ + Rotation(const std::int8_t port); + + Rotation(const Device& device) : Rotation(device.get_port()){}; + /** * Reset the Rotation Sensor @@ -68,7 +71,7 @@ class Rotation : public Device { * * \return 1 if the operation was successful or PROS_ERR if the operation * failed, setting errno. - * + * * \b Example * \code * void opcontrol() { @@ -105,14 +108,14 @@ class Rotation : public Device { * \param rate The data refresh interval in milliseconds * \return 1 if the operation was successful or PROS_ERR if the operation * failed, setting errno. - * + * * \b Example - * \code - * void initialize() { - * pros::Rotation rotation_sensor(1); - * rotation_sensor.set_data_rate(5); - * } - * \endcode + * \code + * void initialize() { + * pros::Rotation rotation_sensor(1); + * rotation_sensor.set_data_rate(5); + * } + * \endcode */ virtual std::int32_t set_data_rate(std::uint32_t rate) const; @@ -128,7 +131,7 @@ class Rotation : public Device { * The position in terms of ticks * \return 1 if the operation was successful or PROS_ERR if the operation * failed, setting errno. - * + * * \b Example * \code * void opcontrol() { @@ -157,7 +160,7 @@ class Rotation : public Device { * The position in terms of ticks * \return 1 if the operation was successful or PROS_ERR if the operation * failed, setting errno. - * + * * \b Example * \code * void opcontrol() { @@ -174,6 +177,20 @@ class Rotation : public Device { */ virtual std::int32_t reset_position(void) const; + /** + * Gets all rotation sensors. + * + * \return A vector of Rotation sensor objects. + * + * \b Example + * \code + * void opcontrol() { + * std::vector rotation_all = pros::Rotation::get_all_devices(); // All rotation sensors that are connected + * } + * \endcode + */ + static std::vector get_all_devices(); + /** * Get the Rotation Sensor's current position in centidegrees * @@ -184,17 +201,17 @@ class Rotation : public Device { * * \return The position value or PROS_ERR if the operation failed, setting * errno. - * + * * \b Example - * \code - * void opcontrol() { + * \code + * void opcontrol() { * pros::Rotation rotation_sensor(1); - * while (true) { - * printf("Position: %d Ticks \n", rotation_sensor.get_position()); - * delay(20); - * } - * } - * \endcode + * while (true) { + * printf("Position: %d Ticks \n", rotation_sensor.get_position()); + * delay(20); + * } + * } + * \endcode */ virtual std::int32_t get_position() const; @@ -210,17 +227,17 @@ class Rotation : public Device { * The V5 Rotation Sensor port number from 1-21 * \return The velocity value or PROS_ERR if the operation failed, setting * errno. - * + * * \b Example - * \code - * void opcontrol() { + * \code + * void opcontrol() { * pros::Rotation rotation_sensor(1); - * while (true) { - * printf("Velocity: %d centidegrees per second \n", rotation_sensor.get_velocity)); - * delay(20); - * } - * } - * \endcode + * while (true) { + * printf("Velocity: %d centidegrees per second \n", rotation_sensor.get_velocity)); + * delay(20); + * } + * } + * \endcode */ virtual std::int32_t get_velocity() const; @@ -234,17 +251,17 @@ class Rotation : public Device { * * \return The angle value or PROS_ERR if the operation failed, setting * errno. - * + * * \b Example - * \code - * void opcontrol() { + * \code + * void opcontrol() { * pros::Rotation rotation_sensor(1); - * while (true) { - * printf("Angle: %d centidegrees \n", rotation_sensor.get_angle()); - * delay(20); - * } - * } - * \endcode + * while (true) { + * printf("Angle: %d centidegrees \n", rotation_sensor.get_angle()); + * delay(20); + * } + * } + * \endcode */ virtual std::int32_t get_angle() const; @@ -262,7 +279,7 @@ class Rotation : public Device { * * \return 1 if the operation was successful or PROS_ERR if the operation * failed, setting errno. - * + * * \b Example * \code * void opcontrol() { @@ -289,7 +306,7 @@ class Rotation : public Device { * * \return 1 if the operation was successful or PROS_ERR if the operation * failed, setting errno. - * + * * \b Example * \code * void opcontrol() { @@ -316,27 +333,27 @@ class Rotation : public Device { * * \return Reversed value or PROS_ERR if the operation failed, setting * errno. - * + * * \b Example - * \code - * void opcontrol() { + * \code + * void opcontrol() { * pros::Rotation rotation_sensor(1); - * while (true) { - * printf("Reversed: %d \n", rotation_sensor.get_reversed()); - * delay(20); - * } - * } - * \endcode + * while (true) { + * printf("Reversed: %d \n", rotation_sensor.get_reversed()); + * delay(20); + * } + * } + * \endcode */ virtual std::int32_t get_reversed() const; /** * This is the overload for the << operator for printing to streams - * + * * Prints in format(this below is all in one line with no new line): - * Rotation [port: rotation._port, position: (rotation position), velocity: (rotation velocity), + * Rotation [port: rotation._port, position: (rotation position), velocity: (rotation velocity), * angle: (rotation angle), reversed: (reversed boolean)] - * + * * \b Example * \code * #define ROTATION_PORT 1 @@ -352,13 +369,26 @@ class Rotation : public Device { */ friend std::ostream& operator<<(std::ostream& os, const pros::Rotation& rotation); -///@} + ///@} }; namespace literals { +/** + * Constructs a Rotation sensor from a literal ending in _rot + * + * \return a pros::Rotation for the corresponding port + * + * \b Example + * \code + * using namespace pros::literals; + * void opcontrol() { + * pros::Rotation rotation = 2_rot; //Makes an Motor object on port 2 + * } + * \endcode + */ const pros::Rotation operator"" _rot(const unsigned long long int r); } // namespace literals -} +} // namespace v5 } // namespace pros #endif diff --git a/include/pros/screen.h b/include/pros/screen.h index 8799058ee..71c285701 100644 --- a/include/pros/screen.h +++ b/include/pros/screen.h @@ -79,16 +79,11 @@ typedef enum { * Struct representing screen touch status, screen last x, screen last y, press count, release count. */ typedef struct screen_touch_status_s { - /// Represents if the screen is being held, released, or pressed. - last_touch_e_t touch_status; - /// Represents the x value of the location of the touch. - int16_t x; - /// Represents the y value of the location of the touch. - int16_t y; - /// Represents how many times the screen has be pressed. - int32_t press_count; - /// Represents how many times the user released after a touch on the screen. - int32_t release_count; + last_touch_e_t touch_status; ///< Represents if the screen is being held, released, or pressed. + int16_t x; ///< Represents the x value of the location of the touch. + int16_t y; ///< Represents the y value of the location of the touch. + int32_t press_count; ///< Represents how many times the screen has be pressed. + int32_t release_count; ///< Represents how many times the user released after a touch on the screen. } screen_touch_status_s_t; #ifdef PROS_USE_SIMPLE_NAMES diff --git a/include/pros/serial.hpp b/include/pros/serial.hpp index a2490e236..3880f61b7 100644 --- a/include/pros/serial.hpp +++ b/include/pros/serial.hpp @@ -12,7 +12,7 @@ * This Source Code Form is subject to the terms of the Mozilla Public * License, v. 2.0. If a copy of the MPL was not distributed with this * file, You can obtain one at http://mozilla.org/MPL/2.0/. - * + * * \defgroup cpp-serial Generic Serial C++ API */ @@ -20,8 +20,9 @@ #define _PROS_SERIAL_HPP_ #include -#include "pros/serial.h" + #include "pros/device.hpp" +#include "pros/serial.h" namespace pros { /** @@ -46,8 +47,8 @@ class Serial : public Device { * The V5 port number from 1-21 * \param baudrate * The baudrate to run the port at - * - * \b Example: + * + * \b Example: * \code * pros::Serial serial(1, 9600); * \endcode @@ -91,7 +92,7 @@ class Serial : public Device { * * \return 1 if the operation was successful or PROS_ERR if the operation * failed, setting errno. - * + * * \b Example: * \code * pros::Serial serial(1); @@ -142,7 +143,7 @@ class Serial : public Device { * * \return The number of bytes avaliable to be read or PROS_ERR if the operation * failed, setting errno. - * + * * \b Example: * \code * void opcontrol() { @@ -192,7 +193,7 @@ class Serial : public Device { * * \return The next byte avaliable to be read, -1 if none are available, or * PROS_ERR if the operation failed, setting errno. - * + * * \b Example: * \code * void opcontrol() { @@ -318,12 +319,25 @@ class Serial : public Device { * \endcode */ virtual std::int32_t write(std::uint8_t* buffer, std::int32_t length) const; - + private: ///@} }; namespace literals { +/** + * Constructs a Serial device from a litteral ending in _ser + * + * \return a pros::Serial for the corresponding port + * + * \b Example + * \code + * using namespace pros::literals; + * void opcontrol() { + * pros::Serial serial = 2_ser; //Makes an Serial device object on port 2 + * } + * \endcode + */ const pros::Serial operator"" _ser(const unsigned long long int m); } // namespace literals } // namespace pros diff --git a/include/pros/vision.hpp b/include/pros/vision.hpp index b8676794a..ab4a61d65 100644 --- a/include/pros/vision.hpp +++ b/include/pros/vision.hpp @@ -23,6 +23,7 @@ #include +#include "pros/device.hpp" #include "pros/vision.h" namespace pros { @@ -48,7 +49,7 @@ class Vision : public Device { * The V5 port number from 1-21 * \param zero_point * One of vision_zero_e_t to set the (0,0) coordinate for the FOV - * + * * \b Example * \code * void opcontrol() { @@ -56,7 +57,9 @@ class Vision : public Device { * } * \endcode */ - explicit Vision(std::uint8_t port, vision_zero_e_t zero_point = E_VISION_ZERO_TOPLEFT); + Vision(std::uint8_t port, vision_zero_e_t zero_point = E_VISION_ZERO_TOPLEFT); + + Vision(const Device& device) : Vision(device.get_port()){}; /** * Clears the vision sensor LED color, reseting it back to its default @@ -68,14 +71,14 @@ class Vision : public Device { * * \return 1 if the operation was successful or PROS_ERR if the operation * failed, setting errno. - * + * * \b Example - * \code - * void initialize() { + * \code + * void initialize() { * pros::Vision vision_sensor(1); - * vision_sensor.clear_led(); - * } - * \endcode + * vision_sensor.clear_led(); + * } + * \endcode */ std::int32_t clear_led(void) const; @@ -102,27 +105,27 @@ class Vision : public Device { * Signature type * * \return A vision_signature_s_t that can be set using Vision::set_signature - * + * * \b Example - * \code - * #define VISION_PORT 1 - * #define EXAMPLE_SIG 1 - * - * void opcontrol() { + * \code + * #define VISION_PORT 1 + * #define EXAMPLE_SIG 1 + * + * void opcontrol() { * pros::Vision vision_sensor(VISION_PORT); - * // values acquired from the vision utility - * vision_signature_s_t RED_SIG = - * vision_signature_from_utility(EXAMPLE_SIG, 8973, 11143, 10058, -2119, -1053, -1586, 5.4, 0); - * vision_sensor.set_signature(EXAMPLE_SIG, &RED_SIG); - * while (true) { - * vision_signature_s_t rtn = vision_sensor.get_by_sig(VISION_PORT, 0, EXAMPLE_SIG); - * // Gets the largest object of the EXAMPLE_SIG signature - * printf("sig: %d", rtn.signature); - * // Prints "sig: 1" - * delay(2); - * } - * } - * \endcode + * // values acquired from the vision utility + * vision_signature_s_t RED_SIG = + * vision_signature_from_utility(EXAMPLE_SIG, 8973, 11143, 10058, -2119, -1053, -1586, 5.4, 0); + * vision_sensor.set_signature(EXAMPLE_SIG, &RED_SIG); + * while (true) { + * vision_signature_s_t rtn = vision_sensor.get_by_sig(VISION_PORT, 0, EXAMPLE_SIG); + * // Gets the largest object of the EXAMPLE_SIG signature + * printf("sig: %d", rtn.signature); + * // Prints "sig: 1" + * delay(2); + * } + * } + * \endcode */ static vision_signature_s_t signature_from_utility(const std::int32_t id, const std::int32_t u_min, const std::int32_t u_max, const std::int32_t u_mean, @@ -151,23 +154,37 @@ class Vision : public Device { * The fifth signature id [1-7] to add to the color code * * \return A vision_color_code_t object containing the color code information. - * + * * \b Example - * \code - * #define VISION_PORT 1 - * #define EXAMPLE_SIG 1 - * #define OTHER_SIG 2 - * - * void opcontrol() { + * \code + * #define VISION_PORT 1 + * #define EXAMPLE_SIG 1 + * #define OTHER_SIG 2 + * + * void opcontrol() { * pros::Vision vision_sensor(VISION_PORT); - * vision_color_code_t code1 = vision_sensor.create_color_code(EXAMPLE_SIG, OTHER_SIG); - * } - * \endcode + * vision_color_code_t code1 = vision_sensor.create_color_code(EXAMPLE_SIG, OTHER_SIG); + * } + * \endcode */ vision_color_code_t create_color_code(const std::uint32_t sig_id1, const std::uint32_t sig_id2, const std::uint32_t sig_id3 = 0, const std::uint32_t sig_id4 = 0, const std::uint32_t sig_id5 = 0) const; + /** + * Gets all vision sensors. + * + * \return A vector of Vision sensor objects. + * + * \b Example + * \code + * void opcontrol() { + * std::vector vision_all = pros::Vision::get_all_devices(); // All vision sensors that are connected + * } + * \endcode + */ + static std::vector get_all_devices(); + /** * Gets the nth largest object according to size_id. * @@ -183,21 +200,21 @@ class Vision : public Device { * * \return The vision_object_s_t object corresponding to the given size id, or * PROS_ERR if an error occurred. - * + * * \b Example - * \code - * #define VISION_PORT 1 - * - * void opcontrol() { + * \code + * #define VISION_PORT 1 + * + * void opcontrol() { * pros::Vision vision_sensor(VISION_PORT); - * while (true) { - * vision_object_s_t rtn = vision_sensor.get_by_size(0); - * // Gets the largest object - * printf("sig: %d", rtn.signature); - * delay(2); - * } - * } - * \endcode + * while (true) { + * vision_object_s_t rtn = vision_sensor.get_by_size(0); + * // Gets the largest object + * printf("sig: %d", rtn.signature); + * delay(2); + * } + * } + * \endcode */ vision_object_s_t get_by_size(const std::uint32_t size_id) const; @@ -220,23 +237,23 @@ class Vision : public Device { * * \return The vision_object_s_t object corresponding to the given signature * and size_id, or PROS_ERR if an error occurred. - * + * * \b Example - * \code - * #define VISION_PORT 1 - * #define EXAMPLE_SIG 1 - * - * void opcontrol() { + * \code + * #define VISION_PORT 1 + * #define EXAMPLE_SIG 1 + * + * void opcontrol() { * pros::Vision vision_sensor(VISION_PORT); - * while (true) { - * vision_object_s_t rtn = vision_sensor.get_by_sig(0, EXAMPLE_SIG); - * // Gets the largest object of the EXAMPLE_SIG signature - * printf("sig: %d", rtn.signature); - * // Prints "sig: 1" - * delay(2); - * } - * } - * \endcode + * while (true) { + * vision_object_s_t rtn = vision_sensor.get_by_sig(0, EXAMPLE_SIG); + * // Gets the largest object of the EXAMPLE_SIG signature + * printf("sig: %d", rtn.signature); + * // Prints "sig: 1" + * delay(2); + * } + * } + * \endcode */ vision_object_s_t get_by_sig(const std::uint32_t size_id, const std::uint32_t sig_id) const; @@ -256,29 +273,29 @@ class Vision : public Device { * * \return The vision_object_s_t object corresponding to the given color code * and size_id, or PROS_ERR if an error occurred. - * + * * \b Example - * \code - * #define VISION_PORT 1 - * #define EXAMPLE_SIG 1 - * #define OTHER_SIG 2 - * - * void opcontrol() { + * \code + * #define VISION_PORT 1 + * #define EXAMPLE_SIG 1 + * #define OTHER_SIG 2 + * + * void opcontrol() { * pros::Vision vision_sensor(VISION_PORT); - * vision_color_code_t code1 = vision_sensor.create_color_code(EXAMPLE_SIG, OTHER_SIG); - * while (true) { - * vision_object_s_t rtn = vision_sensor.get_by_code(0, code1); - * // Gets the largest object - * printf("sig: %d", rtn.signature); - * delay(2); - * } - * } - * \endcode + * vision_color_code_t code1 = vision_sensor.create_color_code(EXAMPLE_SIG, OTHER_SIG); + * while (true) { + * vision_object_s_t rtn = vision_sensor.get_by_code(0, code1); + * // Gets the largest object + * printf("sig: %d", rtn.signature); + * delay(2); + * } + * } + * \endcode */ vision_object_s_t get_by_code(const std::uint32_t size_id, const vision_color_code_t color_code) const; /** - * Gets the exposure parameter of the Vision Sensor. + * Gets the exposure parameter of the Vision Sensor. * * This function uses the following values of errno when an error state is * reached: @@ -286,17 +303,17 @@ class Vision : public Device { * * \return The current exposure parameter from [0,150], * PROS_ERR if an error occurred - * + * * \b Example - * \code - * #define VISION_PORT 1 - * - * void initialize() { + * \code + * #define VISION_PORT 1 + * + * void initialize() { * pros::Vision vision_sensor(VISION_PORT); - * if (vision_sensor.get_exposure() < 50) - * vision_sensor.set_exposure(50); - * } - * \endcode + * if (vision_sensor.get_exposure() < 50) + * vision_sensor.set_exposure(50); + * } + * \endcode */ std::int32_t get_exposure(void) const; @@ -309,19 +326,19 @@ class Vision : public Device { * * \return The number of objects detected on the specified vision sensor. * Returns PROS_ERR if the port was invalid or an error occurred. - * + * * \b Example - * \code - * #define VISION_PORT 1 - * - * void opcontrol() { + * \code + * #define VISION_PORT 1 + * + * void opcontrol() { * pros::Vision vision_sensor(VISION_PORT); - * while (true) { - * printf("Number of Objects Detected: %d\n", vision_sensor.get_object_count()); - * delay(2); - * } - * } - * \endcode + * while (true) { + * printf("Number of Objects Detected: %d\n", vision_sensor.get_object_count()); + * delay(2); + * } + * } + * \endcode */ std::int32_t get_object_count(void) const; @@ -336,18 +353,18 @@ class Vision : public Device { * The signature id to read * * \return A vision_signature_s_t containing information about the signature. - * + * * \b Example - * \code - * #define VISION_PORT 1 - * #define EXAMPLE_SIG 1 - * - * void opcontrol() { + * \code + * #define VISION_PORT 1 + * #define EXAMPLE_SIG 1 + * + * void opcontrol() { * pros::Vision vision_sensor(VISION_PORT); - * vision_signature_s_t sig = vision_sensor.get_signature(EXAMPLE_SIG); - * vision_sensor.print_signature(sig); - * } - * \endcode + * vision_signature_s_t sig = vision_sensor.get_signature(EXAMPLE_SIG); + * vision_sensor.print_signature(sig); + * } + * \endcode */ vision_signature_s_t get_signature(const std::uint8_t signature_id) const; @@ -359,22 +376,21 @@ class Vision : public Device { * ENODEV - The port cannot be configured as a vision sensor * * \return The current RGB white balance setting of the sensor - * + * * \b Example - * \code - * #define VISION_PORT 1 - * #define VISION_WHITE 0xff - * - * void initialize() { + * \code + * #define VISION_PORT 1 + * #define VISION_WHITE 0xff + * + * void initialize() { * pros::Vision vision_sensor(VISION_PORT); - * if (vision_sensor.get_white_balance() != VISION_WHITE) - * vision_sensor.set_white_balance(VISION_WHITE); - * } - * \endcode + * if (vision_sensor.get_white_balance() != VISION_WHITE) + * vision_sensor.set_white_balance(VISION_WHITE); + * } + * \endcode */ std::int32_t get_white_balance(void) const; - /** * Reads up to object_count object descriptors into object_arr. * @@ -397,23 +413,23 @@ class Vision : public Device { * Returns PROS_ERR if the port was invalid, an error occurred, or fewer objects * than size_id were found. All objects in object_arr that were not found are * given VISION_OBJECT_ERR_SIG as their signature. - * + * * \b Example - * \code - * #define VISION_PORT 1 - * #define NUM_VISION_OBJECTS 4 - * - * void opcontrol() { + * \code + * #define VISION_PORT 1 + * #define NUM_VISION_OBJECTS 4 + * + * void opcontrol() { * pros::Vision vision_sensor(VISION_PORT); - * vision_object_s_t object_arr[NUM_VISION_OBJECTS]; - * while (true) { - * vision_sensor.read_by_size(0, NUM_VISION_OBJECTS, object_arr); - * printf("sig: %d", object_arr[0].signature); - * // Prints the signature of the largest object found - * delay(2); - * } - * } - * \endcode + * vision_object_s_t object_arr[NUM_VISION_OBJECTS]; + * while (true) { + * vision_sensor.read_by_size(0, NUM_VISION_OBJECTS, object_arr); + * printf("sig: %d", object_arr[0].signature); + * // Prints the signature of the largest object found + * delay(2); + * } + * } + * \endcode */ std::int32_t read_by_size(const std::uint32_t size_id, const std::uint32_t object_count, vision_object_s_t* const object_arr) const; @@ -444,19 +460,19 @@ class Vision : public Device { * Returns PROS_ERR if the port was invalid, an error occurred, or fewer objects * than size_id were found. All objects in object_arr that were not found are * given VISION_OBJECT_ERR_SIG as their signature. - * + * * \b Example - * \code - * #define VISION_PORT 1 - * #define EXAMPLE_SIG 1 - * #define NUM_VISION_OBJECTS 4 - * - * void opcontrol() { + * \code + * #define VISION_PORT 1 + * #define EXAMPLE_SIG 1 + * #define NUM_VISION_OBJECTS 4 + * + * void opcontrol() { * pros::Vision vision_sensor(VISION_PORT); - * vision_object_s_t object_arr[NUM_VISION_OBJECTS]; - * while (true) { - * vision_sensor.read_by_sig(0, EXAMPLE_SIG, NUM_VISION_OBJECTS, object_arr); - * printf("sig: %d", object_arr[0].signature); + * vision_object_s_t object_arr[NUM_VISION_OBJECTS]; + * while (true) { + * vision_sensor.read_by_sig(0, EXAMPLE_SIG, NUM_VISION_OBJECTS, object_arr); + * printf("sig: %d", object_arr[0].signature); * // Prints "sig: 1" * delay(2); * } @@ -490,14 +506,14 @@ class Vision : public Device { * Returns PROS_ERR if the port was invalid, an error occurred, or fewer objects * than size_id were found. All objects in object_arr that were not found are * given VISION_OBJECT_ERR_SIG as their signature. - * + * * \b Example - * \code + * \code * #define VISION_PORT 1 * #define EXAMPLE_SIG 1 * #define OTHER_SIG 2 * #define NUM_VISION_OBJECTS 4 - * + * * void opcontrol() { * pros::Vision vision_sensor(VISION_PORT); * vision_object_s_t object_arr[NUM_VISION_OBJECTS]; @@ -521,12 +537,12 @@ class Vision : public Device { * The signature for which the contents will be printed * * \return 1 if no errors occured, PROS_ERR otherwise - * + * * \b Example * \code - * #define VISION_PORT 1 + * #define VISION_PORT 1 * #define EXAMPLE_SIG 1 - * + * * void opcontrol() { * pros::Vision vision_sensor(VISION_PORT); * vision_signature_s_t sig = visionsensor.get_signature(EXAMPLE_SIG); @@ -548,11 +564,11 @@ class Vision : public Device { * * \return 1 if the operation was successful or PROS_ERR if the operation * failed, setting errno. - * + * * \b Example - * \code - * #define VISION_PORT 1 - * + * \code + * #define VISION_PORT 1 + * * void initialize() { * pros::Vision vision_sensor(VISION_PORT); * vision_sensor.set_auto_white_balance(true); @@ -562,7 +578,7 @@ class Vision : public Device { std::int32_t set_auto_white_balance(const std::uint8_t enable) const; /** - * Sets the exposure parameter of the Vision Sensor. + * Sets the exposure parameter of the Vision Sensor. * * This function uses the following values of errno when an error state is * reached: @@ -573,11 +589,11 @@ class Vision : public Device { * * \return 1 if the operation was successful or PROS_ERR if the operation * failed, setting errno. - * + * * \b Example * \code * #define VISION_PORT 1 - * + * * void initialize() { * pros::Vision vision_sensor(VISION_PORT); * if (vision_sensor.get_exposure() < 50) @@ -599,11 +615,11 @@ class Vision : public Device { * * \return 1 if the operation was successful or PROS_ERR if the operation * failed, setting errno. - * + * * \b Example * \code * #define VISION_PORT 1 - * + * * void initialize() { * pros::Vision vision_sensor(VISION_PORT); * vision_sensor.set_led(COLOR_BLANCHED_ALMOND); @@ -629,12 +645,12 @@ class Vision : public Device { * A pointer to the signature to save * * \return 1 if no errors occured, PROS_ERR otherwise - * + * * \b Example * \code * #define VISION_PORT 1 * #define EXAMPLE_SIG 1 - * + * * void opcontrol() { * pros::Vision vision_sensor(VISION_PORT); * vision_signature_s_t sig = vision_sensor.get_signature(EXAMPLE_SIG); @@ -657,12 +673,12 @@ class Vision : public Device { * * \return 1 if the operation was successful or PROS_ERR if the operation * failed, setting errno. - * + * * \b Example - * \code + * \code * #define VISION_PORT 1 * #define VISION_WHITE 0xff - * + * * void initialize() { * pros::Vision vision_sensor(VISION_PORT); * vision_sensor.set_white_balance(VISION_WHITE); @@ -687,11 +703,11 @@ class Vision : public Device { * * \return 1 if the operation was successful or PROS_ERR if the operation * failed, setting errno. - * + * * \b Example - * \code + * \code * #define VISION_PORT 1 - * + * * void initialize() { * pros::Vision vision_sensor(VISION_PORT); * vision_sensor.set_zero_point(E_VISION_ZERO_CENTER); @@ -712,22 +728,60 @@ class Vision : public Device { * * \return 1 if the operation was successful or PROS_ERR if the operation * failed, setting errno. - * + * * \b Example - * \code - * #define VISION_PORT 1 - * - * void initialize() { + * \code + * #define VISION_PORT 1 + * + * void initialize() { * pros::Vision vision_sensor(VISION_PORT); - * vision_sensor.set_wifi_mode(0); - * } - * \endcode + * vision_sensor.set_wifi_mode(0); + * } + * \endcode */ std::int32_t set_wifi_mode(const std::uint8_t enable) const; - + + /** + * Gets a vision sensor that is plugged in to the brain + * + * \note The first time this function is called it returns the vision sensor at the lowest port + * If this function is called multiple times, it will cycle through all the ports. + * For example, if you have 1 vision sensor on the robot + * this function will always return a vision sensor object for that port. + * If you have 2 vision sensors, all the odd numered calls to this function will return objects + * for the lower port number, + * all the even number calls will return vision objects for the higher port number + * + * + * This functions uses the following values of errno when an error state is + * reached: + * ENODEV - No vision sensor is plugged into the brain + * + * \return A vision object corresponding to a port that a vision sensor is connected to the brain + * If no vision sensor is plugged in, it returns a vision sensor on port PROS_ERR_BYTE + * + */ + static Vision get_vision(); + private: ///@} }; } // namespace v5 +namespace literals { +/** + * Constructs a Vision sensor from a litteral ending in _vis + * + * \return a pros::Vision for the corresponding port + * + * \b Example + * \code + * using namespace pros::literals; + * void opcontrol() { + * pros::Vision vision = 2_vis; //Makes an Vision sensor object on port 2 + * } + * \endcode + */ +const pros::Vision operator"" _vis(const unsigned long long int m); +} // namespace literals } // namespace pros #endif // _PROS_VISION_HPP_ diff --git a/src/devices/controller.c b/src/devices/controller.c index c7dea3ce1..2c5e91880 100644 --- a/src/devices/controller.c +++ b/src/devices/controller.c @@ -183,3 +183,23 @@ int32_t controller_rumble(controller_id_e_t id, const char* rumble_pattern) { uint8_t competition_get_status(void) { return vexCompetitionStatus(); } + +uint8_t competition_is_disabled(void) { + return (competition_get_status() & COMPETITION_DISABLED) != 0; +} + +uint8_t competition_is_connected(void) { + return (competition_get_status() & COMPETITION_CONNECTED) != 0; +} + +uint8_t competition_is_autonomous(void) { + return (competition_get_status() & COMPETITION_AUTONOMOUS) != 0; +} + +uint8_t competition_is_field(void) { + return ((competition_get_status() & COMPETITION_SYSTEM) != 0) && competition_is_connected(); +} + +uint8_t competition_is_switch(void) { + return ((competition_get_status() & COMPETITION_SYSTEM) == 0) && competition_is_connected(); +} diff --git a/src/devices/controller.cpp b/src/devices/controller.cpp index 989b890e1..0adfe7dce 100644 --- a/src/devices/controller.cpp +++ b/src/devices/controller.cpp @@ -67,6 +67,7 @@ std::int32_t Controller::rumble(const char* rumble_pattern) { } // namespace v5 namespace competition { +using namespace pros::c; std::uint8_t get_status(void) { return competition_get_status(); } @@ -82,5 +83,14 @@ std::uint8_t is_connected(void) { std::uint8_t is_disabled(void) { return competition_is_disabled(); } + +std::uint8_t is_field_control(void) { + return competition_is_field(); +} + +std::uint8_t is_competition_switch(void) { + return competition_is_switch(); +} + } // namespace competition } // namespace pros diff --git a/src/devices/vdml_adi.cpp b/src/devices/vdml_adi.cpp index 607163c78..5f3ed46c8 100644 --- a/src/devices/vdml_adi.cpp +++ b/src/devices/vdml_adi.cpp @@ -46,6 +46,10 @@ std::int32_t Port::get_value() const { return ext_adi_port_get_value(_smart_port, _adi_port); } +ext_adi_port_tuple_t Port::get_port() const { + return std::make_tuple(_smart_port, _adi_port, PROS_ERR_BYTE); +} + AnalogIn::AnalogIn(std::uint8_t adi_port) : Port(adi_port, E_ADI_ANALOG_IN) {} AnalogIn::AnalogIn(ext_adi_port_pair_t port_pair) : Port(port_pair, E_ADI_ANALOG_IN) {} @@ -63,8 +67,8 @@ std::int32_t AnalogIn::get_value_calibrated_HR() const { std::ostream& operator<<(std::ostream& os, pros::adi::AnalogIn& analog_in) { os << "AnalogIn ["; - os << "smart_port: " << analog_in._smart_port; - os << ", adi_port: " << analog_in._adi_port; + os << "smart_port: " << +analog_in._smart_port; + os << ", adi_port: " << ((analog_in._adi_port > 10) ? analog_in._adi_port : +analog_in._adi_port); os << ", value calibrated: " << analog_in.get_value_calibrated(); os << ", value calibrated HR: " << analog_in.get_value_calibrated_HR(); os << ", value: " << analog_in.get_value(); @@ -78,8 +82,8 @@ AnalogOut::AnalogOut(ext_adi_port_pair_t port_pair) : Port(port_pair, E_ADI_ANAL std::ostream& operator<<(std::ostream& os, pros::adi::AnalogOut& analog_out) { os << "AnalogOut ["; - os << "smart_port: " << analog_out._smart_port; - os << ", adi_port: " << analog_out._adi_port; + os << "smart_port: " << +analog_out._smart_port; + os << ", adi_port: " << ((analog_out._adi_port > 10) ? analog_out._adi_port : +analog_out._adi_port); os << ", value: " << analog_out.get_value(); os << "]"; @@ -95,8 +99,8 @@ std::int32_t DigitalIn::get_new_press() const { std::ostream& operator<<(std::ostream& os, pros::adi::DigitalIn& digital_in) { os << "DigitalIn ["; - os << "smart_port: " << digital_in._smart_port; - os << ", adi_port: " << digital_in._adi_port; + os << "smart_port: " << +digital_in._smart_port; + os << ", adi_port: " << ((digital_in._adi_port > 10) ? digital_in._adi_port : +digital_in._adi_port); os << ", value: " << digital_in.get_value(); os << "]"; @@ -113,8 +117,8 @@ DigitalOut::DigitalOut(ext_adi_port_pair_t port_pair, bool init_state) : ADIPort std::ostream& operator<<(std::ostream& os, pros::adi::DigitalOut& digital_out) { os << "DigitalOut ["; - os << "smart_port: " << digital_out._smart_port; - os << ", adi_port: " << digital_out._adi_port; + os << "smart_port: " << +digital_out._smart_port; + os << ", adi_port: " << ((digital_out._adi_port > 10) ? digital_out._adi_port : +digital_out._adi_port); os << ", value: " << digital_out.get_value(); os << "]"; @@ -133,16 +137,15 @@ std::int32_t Motor::stop() const { return ext_adi_motor_stop(_smart_port, _adi_port); } -Encoder::Encoder(std::uint8_t adi_port_top, std::uint8_t adi_port_bottom, bool reversed) : Port(adi_port_top) { +Encoder::Encoder(std::uint8_t adi_port_top, std::uint8_t adi_port_bottom, bool reversed) : Port(adi_port_top), _port_pair(adi_port_top, adi_port_bottom) { std::int32_t _port = ext_adi_encoder_init(INTERNAL_ADI_PORT, adi_port_top, adi_port_bottom, reversed); get_ports(_port, _smart_port, _adi_port); } -Encoder::Encoder(ext_adi_port_tuple_t port_tuple, bool reversed) : Port(std::get<1>(port_tuple)) { +Encoder::Encoder(ext_adi_port_tuple_t port_tuple, bool reversed) : Port(std::get<1>(port_tuple)), _port_pair(std::get<1>(port_tuple), std::get<2>(port_tuple)) { std::int32_t _port = ext_adi_encoder_init(std::get<0>(port_tuple), std::get<1>(port_tuple), std::get<2>(port_tuple), reversed); get_ports(_port, _smart_port, _adi_port); - } std::int32_t Encoder::reset() const { @@ -153,10 +156,14 @@ std::int32_t Encoder::get_value() const { return ext_adi_encoder_get(merge_adi_ports(_smart_port, _adi_port)); } +ext_adi_port_tuple_t ADIEncoder::get_port() const { + return std::make_tuple(_smart_port, std::get<0>(_port_pair), std::get<1>(_port_pair)); +} + std::ostream& operator<<(std::ostream& os, pros::adi::Encoder& encoder) { os << "Encoder ["; - os << "smart_port: " << encoder._smart_port; - os << ", adi_port: " << encoder._adi_port; + os << "smart_port: " << +encoder._smart_port; + os << ", adi_port: " << ((encoder._adi_port > 10) ? encoder._adi_port : +encoder._adi_port); os << ", value: " << encoder.get_value(); os << "]"; @@ -199,13 +206,11 @@ std::int32_t Gyro::reset() const { Potentiometer::Potentiometer(std::uint8_t adi_port, adi_potentiometer_type_e_t potentiometer_type) : AnalogIn(adi_port) { std::int32_t _port = ext_adi_potentiometer_init(INTERNAL_ADI_PORT, adi_port, potentiometer_type); get_ports(_port, _smart_port, _adi_port); - _smart_port++; // for inherited functions this is necessary } Potentiometer::Potentiometer(ext_adi_port_pair_t port_pair, adi_potentiometer_type_e_t potentiometer_type) : AnalogIn(std::get<1>(port_pair)) { std::int32_t _port = ext_adi_potentiometer_init(port_pair.first, port_pair.second, potentiometer_type); get_ports(_port, _smart_port, _adi_port); - _smart_port++; // for inherited functions this is necessary } double Potentiometer::get_angle() const { @@ -312,7 +317,9 @@ bool Pneumatics::is_extended() const { std::ostream& operator<<(std::ostream& os, pros::adi::Potentiometer& potentiometer) { os << "Potentiometer ["; - os << "value: " << potentiometer.get_value(); + os << "smart_port: " << +potentiometer._smart_port; + os << ", adi_port: " << ((potentiometer._adi_port > 10) ? potentiometer._adi_port : +potentiometer._adi_port); + os << ", value: " << potentiometer.get_value(); os << ", value calibrated: " << potentiometer.get_value_calibrated(); os << ", angle: " << potentiometer.get_angle(); os << "]"; diff --git a/src/devices/vdml_device.cpp b/src/devices/vdml_device.cpp index 027f6fb1d..682b4245a 100644 --- a/src/devices/vdml_device.cpp +++ b/src/devices/vdml_device.cpp @@ -28,18 +28,40 @@ bool Device::is_installed() { return_port(zero_indexed_port, _deviceType == plugged_device_type); } -std::uint8_t Device::get_port(void) { +std::uint8_t Device::get_port(void) const { return _port; } pros::DeviceType Device::get_plugged_type() const { - if (!port_mutex_take(_port - 1)) { + return(get_plugged_type(_port)); +} + +pros::DeviceType Device::get_plugged_type(std::uint8_t port) { + if (!port_mutex_take(port - 1)) { errno = EACCES; return DeviceType::undefined; } - DeviceType type = (DeviceType) pros::c::registry_get_plugged_type(_port - 1); + DeviceType type = (DeviceType) pros::c::registry_get_plugged_type(port - 1); - return_port(_port - 1, type); + return_port(port - 1, type); +} + +std::vector Device::get_all_devices(pros::DeviceType device_type) { + std::vector device_list {}; + + for (std::uint8_t curr_port = 0; curr_port < 21; ++curr_port) { + if (!port_mutex_take(curr_port)) { + errno = EACCES; + continue; + } + + pros::DeviceType type = (DeviceType) pros::c::registry_get_plugged_type(curr_port); + if (device_type == type) {; + device_list.push_back(Device {static_cast(curr_port + 1)}); + } + port_mutex_give(curr_port); + } + return device_list; } Device::Device(const std::uint8_t port) : _port(port) {} diff --git a/src/devices/vdml_distance.cpp b/src/devices/vdml_distance.cpp index fa6608cc2..51a89ae17 100644 --- a/src/devices/vdml_distance.cpp +++ b/src/devices/vdml_distance.cpp @@ -22,6 +22,19 @@ std::int32_t Distance::get() { return pros::c::distance_get(_port); } +std::int32_t Distance::get_distance() { + return get(); +} + +std::vector Distance::get_all_devices() { + std::vector matching_devices {Device::get_all_devices(DeviceType::distance)}; + std::vector return_vector; + for (auto device : matching_devices) { + return_vector.push_back(device); + } + return return_vector; +} + std::int32_t Distance::get_confidence() { return pros::c::distance_get_confidence(_port); } diff --git a/src/devices/vdml_ext_adi.c b/src/devices/vdml_ext_adi.c index 575cee04d..2d7316832 100644 --- a/src/devices/vdml_ext_adi.c +++ b/src/devices/vdml_ext_adi.c @@ -50,14 +50,15 @@ typedef union adi_data { } gyro_data; } adi_data_s_t; - // These 2 functions aren't in v5_api.h but should be... so we're going to directly expose them with an extern "C". #ifdef __cplusplus extern "C" { #endif - // private addressable LED API - int32_t vexDeviceAdiAddrLedSet( V5_DeviceT device, uint32_t port, uint32_t *pData, uint32_t nOffset, uint32_t nLength, uint32_t options ); - int32_t vexAdiAddrLedSet( uint32_t index, uint32_t port, uint32_t *pData, uint32_t nOffset, uint32_t nLength, uint32_t options ); +// private addressable LED API +int32_t vexDeviceAdiAddrLedSet(V5_DeviceT device, uint32_t port, uint32_t* pData, uint32_t nOffset, uint32_t nLength, + uint32_t options); +int32_t vexAdiAddrLedSet(uint32_t index, uint32_t port, uint32_t* pData, uint32_t nOffset, uint32_t nLength, + uint32_t options); #ifdef __cplusplus } #endif @@ -74,26 +75,26 @@ extern "C" { return PROS_ERR; \ } -#define validate_type(device, adi_port, smart_port, type) \ - adi_port_config_e_t config = (adi_port_config_e_t)vexDeviceAdiPortConfigGet(device->device_info, adi_port); \ - if (config != type) { \ - errno = EADDRINUSE; \ - printf("Error: validate_type\n"); \ - return_port(smart_port, PROS_ERR); \ +#define validate_type(device, adi_port, smart_port, type) \ + adi_port_config_e_t config = (adi_port_config_e_t)vexDeviceAdiPortConfigGet(device->device_info, adi_port); \ + if (config != type) { \ + errno = EADDRINUSE; \ + printf("Error: validate_type\n"); \ + return_port(smart_port, PROS_ERR); \ } -#define validate_type_f(device, adi_port, smart_port, type) \ +#define validate_type_f(device, adi_port, smart_port, type) \ adi_port_config_e_t config = (adi_port_config_e_t)vexDeviceAdiPortConfigGet(device->device_info, adi_port); \ - if (config != type) { \ - errno = EADDRINUSE; \ - return_port(smart_port, PROS_ERR_F); \ + if (config != type) { \ + errno = EADDRINUSE; \ + return_port(smart_port, PROS_ERR_F); \ } -#define validate_motor(device, adi_port, smart_port) \ +#define validate_motor(device, adi_port, smart_port) \ adi_port_config_e_t config = (adi_port_config_e_t)vexDeviceAdiPortConfigGet(device->device_info, adi_port); \ - if (config != E_ADI_LEGACY_PWM && config != E_ADI_LEGACY_SERVO) { \ - errno = EADDRINUSE; \ - return_port(smart_port, PROS_ERR); \ + if (config != E_ADI_LEGACY_PWM && config != E_ADI_LEGACY_SERVO) { \ + errno = EADDRINUSE; \ + return_port(smart_port, PROS_ERR); \ } /* @@ -282,14 +283,14 @@ ext_adi_encoder_t ext_adi_encoder_init(uint8_t smart_port, uint8_t adi_port_top, adi_data_s_t* const adi_data = &((adi_data_s_t*)(device->pad))[port]; adi_data->encoder_data.reversed = reverse; vexDeviceAdiPortConfigSet(device->device_info, port, E_ADI_LEGACY_ENCODER); - return_port(smart_port - 1, merge_adi_ports(smart_port - 1, port + 1)); + return_port(smart_port - 1, merge_adi_ports(smart_port, port + 1)); } int32_t ext_adi_encoder_get(ext_adi_encoder_t enc) { uint8_t smart_port, adi_port; get_ports(enc, smart_port, adi_port); transform_adi_port(adi_port); - claim_port_i(smart_port, E_DEVICE_ADI); + claim_port_i(smart_port - 1, E_DEVICE_ADI); validate_type(device, adi_port, smart_port - 1, E_ADI_LEGACY_ENCODER); int32_t rtn; @@ -298,29 +299,29 @@ int32_t ext_adi_encoder_get(ext_adi_encoder_t enc) { rtn = -vexDeviceAdiValueGet(device->device_info, adi_port); else rtn = vexDeviceAdiValueGet(device->device_info, adi_port); - return_port(smart_port, rtn); + return_port(smart_port - 1, rtn); } int32_t ext_adi_encoder_reset(ext_adi_encoder_t enc) { uint8_t smart_port, adi_port; get_ports(enc, smart_port, adi_port); transform_adi_port(adi_port); - claim_port_i(smart_port, E_DEVICE_ADI); + claim_port_i(smart_port - 1, E_DEVICE_ADI); validate_type(device, adi_port, smart_port - 1, E_ADI_LEGACY_ENCODER); vexDeviceAdiValueSet(device->device_info, adi_port, 0); - return_port(smart_port, 1); + return_port(smart_port - 1, 1); } int32_t ext_adi_encoder_shutdown(ext_adi_encoder_t enc) { uint8_t smart_port, adi_port; get_ports(enc, smart_port, adi_port); transform_adi_port(adi_port); - claim_port_i(smart_port, E_DEVICE_ADI); + claim_port_i(smart_port - 1, E_DEVICE_ADI); validate_type(device, adi_port, smart_port - 1, E_ADI_LEGACY_ENCODER); vexDeviceAdiPortConfigSet(device->device_info, adi_port, E_ADI_TYPE_UNDEFINED); - return_port(smart_port, 1); + return_port(smart_port - 1, 1); } ext_adi_ultrasonic_t ext_adi_ultrasonic_init(uint8_t smart_port, uint8_t adi_port_ping, uint8_t adi_port_echo) { @@ -331,32 +332,31 @@ ext_adi_ultrasonic_t ext_adi_ultrasonic_init(uint8_t smart_port, uint8_t adi_por errno = EINVAL; return PROS_ERR; } - claim_port_i(smart_port - 1, E_DEVICE_ADI); vexDeviceAdiPortConfigSet(device->device_info, port, E_ADI_LEGACY_ULTRASONIC); - return_port(smart_port - 1, merge_adi_ports(smart_port - 1, port + 1)); + return_port(smart_port - 1, merge_adi_ports(smart_port, port + 1)); } int32_t ext_adi_ultrasonic_get(ext_adi_ultrasonic_t ult) { uint8_t smart_port, adi_port; get_ports(ult, smart_port, adi_port); transform_adi_port(adi_port); - claim_port_i(smart_port, E_DEVICE_ADI); + claim_port_i(smart_port - 1, E_DEVICE_ADI); validate_type(device, adi_port, smart_port - 1, E_ADI_LEGACY_ULTRASONIC); int32_t rtn = vexDeviceAdiValueGet(device->device_info, adi_port); - return_port(smart_port, rtn); + return_port(smart_port - 1, rtn); } int32_t ext_adi_ultrasonic_shutdown(ext_adi_ultrasonic_t ult) { uint8_t smart_port, adi_port; get_ports(ult, smart_port, adi_port); transform_adi_port(adi_port); - claim_port_i(smart_port, E_DEVICE_ADI); + claim_port_i(smart_port - 1, E_DEVICE_ADI); validate_type(device, adi_port, smart_port - 1, E_ADI_LEGACY_ULTRASONIC); vexDeviceAdiPortConfigSet(device->device_info, adi_port, E_ADI_TYPE_UNDEFINED); - return_port(smart_port, 1); + return_port(smart_port - 1, 1); } ext_adi_gyro_t ext_adi_gyro_init(uint8_t smart_port, uint8_t adi_port, double multiplier) { @@ -371,7 +371,7 @@ ext_adi_gyro_t ext_adi_gyro_init(uint8_t smart_port, uint8_t adi_port, double mu adi_port_config_e_t config = vexDeviceAdiPortConfigGet(device->device_info, adi_port); if (config == E_ADI_LEGACY_GYRO) { // Port has already been calibrated, no need to do that again - return_port(smart_port - 1, merge_adi_ports(smart_port - 1, adi_port + 1)); + return_port(smart_port - 1, merge_adi_ports(smart_port, adi_port + 1)); } vexDeviceAdiPortConfigSet(device->device_info, adi_port, E_ADI_LEGACY_GYRO); @@ -381,56 +381,54 @@ ext_adi_gyro_t ext_adi_gyro_init(uint8_t smart_port, uint8_t adi_port, double mu // the calibration time in VexOS. delay(GYRO_CALIBRATION_TIME); } - return_port(smart_port - 1, merge_adi_ports(smart_port - 1, adi_port + 1)); + return_port(smart_port - 1, merge_adi_ports(smart_port, adi_port + 1)); } double ext_adi_gyro_get(ext_adi_gyro_t gyro) { uint8_t smart_port, adi_port; get_ports(gyro, smart_port, adi_port); transform_adi_port(adi_port); - claim_port_f(smart_port, E_DEVICE_ADI); + claim_port_f(smart_port - 1, E_DEVICE_ADI); validate_type_f(device, adi_port, smart_port - 1, E_ADI_LEGACY_GYRO); double rtv = (double)vexDeviceAdiValueGet(device->device_info, adi_port); adi_data_s_t* const adi_data = &((adi_data_s_t*)(device->pad))[adi_port]; rtv -= adi_data->gyro_data.tare_value; rtv *= adi_data->gyro_data.multiplier; - return_port(smart_port, rtv); + return_port(smart_port - 1, rtv); } int32_t ext_adi_gyro_reset(ext_adi_gyro_t gyro) { uint8_t smart_port, adi_port; get_ports(gyro, smart_port, adi_port); transform_adi_port(adi_port); - claim_port_i(smart_port, E_DEVICE_ADI); + claim_port_i(smart_port - 1, E_DEVICE_ADI); validate_type(device, adi_port, smart_port - 1, E_ADI_LEGACY_GYRO); adi_data_s_t* const adi_data = &((adi_data_s_t*)(device->pad))[adi_port]; adi_data->gyro_data.tare_value = vexDeviceAdiValueGet(device->device_info, adi_port); - return_port(smart_port, 1); + return_port(smart_port - 1, 1); } int32_t ext_adi_gyro_shutdown(ext_adi_gyro_t gyro) { uint8_t smart_port, adi_port; get_ports(gyro, smart_port, adi_port); transform_adi_port(adi_port); - claim_port_i(smart_port, E_DEVICE_ADI); + claim_port_i(smart_port - 1, E_DEVICE_ADI); validate_type(device, adi_port, smart_port - 1, E_ADI_LEGACY_GYRO); vexDeviceAdiPortConfigSet(device->device_info, adi_port, E_ADI_TYPE_UNDEFINED); - return_port(smart_port, 1); + return_port(smart_port - 1, 1); } ext_adi_potentiometer_t ext_adi_potentiometer_init(uint8_t smart_port, uint8_t adi_port, adi_potentiometer_type_e_t potentiometer_type) { transform_adi_port(adi_port); claim_port_i(smart_port - 1, E_DEVICE_ADI); - adi_data_s_t* const adi_data = &((adi_data_s_t*)(device->pad))[adi_port]; adi_data->potentiometer_data.potentiometer_type = potentiometer_type; vexDeviceAdiPortConfigSet(device->device_info, adi_port, E_ADI_ANALOG_IN); - - return_port(smart_port - 1, merge_adi_ports(smart_port - 1, adi_port + 1)); + return_port(smart_port - 1, merge_adi_ports(smart_port, adi_port + 1)); } double ext_adi_potentiometer_get_angle(ext_adi_potentiometer_t potentiometer) { @@ -438,7 +436,7 @@ double ext_adi_potentiometer_get_angle(ext_adi_potentiometer_t potentiometer) { uint8_t smart_port, adi_port; get_ports(potentiometer, smart_port, adi_port); transform_adi_port(adi_port); - claim_port_f(smart_port, E_DEVICE_ADI); + claim_port_f(smart_port - 1, E_DEVICE_ADI); validate_type(device, adi_port, smart_port - 1, E_ADI_ANALOG_IN); adi_data_s_t* const adi_data = &((adi_data_s_t*)(device->pad))[adi_port]; @@ -454,53 +452,53 @@ double ext_adi_potentiometer_get_angle(ext_adi_potentiometer_t potentiometer) { errno = ENXIO; rtn = PROS_ERR_F; } - return_port(smart_port, rtn); + return_port(smart_port - 1, rtn); } ext_adi_led_t ext_adi_led_init(uint8_t smart_port, uint8_t adi_port) { transform_adi_port(adi_port); claim_port_i(smart_port - 1, E_DEVICE_ADI); - vexDeviceAdiPortConfigSet(device->device_info, adi_port, (V5_AdiPortConfiguration)E_ADI_DIGITAL_OUT); - return_port(smart_port - 1, merge_adi_ports(smart_port - 1, adi_port + 1)); + vexDeviceAdiPortConfigSet(device->device_info, adi_port, (V5_AdiPortConfiguration)E_ADI_DIGITAL_OUT); + return_port(smart_port - 1, merge_adi_ports(smart_port, adi_port + 1)); } int32_t ext_adi_led_set(ext_adi_led_t led, uint32_t* buffer, uint32_t buffer_length) { uint8_t smart_port, adi_port; get_ports(led, smart_port, adi_port); transform_adi_port(adi_port); - claim_port_i(smart_port, E_DEVICE_ADI); - validate_type(device, adi_port, smart_port, E_ADI_DIGITAL_OUT); + claim_port_i(smart_port - 1, E_DEVICE_ADI); + validate_type(device, adi_port, smart_port - 1, E_ADI_DIGITAL_OUT); if (buffer_length > MAX_LED) { buffer_length = MAX_LED; - } - else if (buffer == NULL || buffer_length < 1) - { + } else if (buffer == NULL || buffer_length < 1) { errno = EINVAL; - return PROS_ERR; + return_port(smart_port - 1, PROS_ERR); } uint32_t rtv = (uint32_t)vexDeviceAdiAddrLedSet(device->device_info, adi_port, buffer, 0, buffer_length, 0); - return_port(smart_port, rtv); + return_port(smart_port - 1, rtv); } -int32_t ext_adi_led_set_pixel(ext_adi_led_t led, uint32_t* buffer, uint32_t buffer_length, uint32_t color, uint32_t pixel_position) { +int32_t ext_adi_led_set_pixel(ext_adi_led_t led, uint32_t* buffer, uint32_t buffer_length, uint32_t color, + uint32_t pixel_position) { uint8_t smart_port, adi_port; get_ports(led, smart_port, adi_port); - claim_port_i(smart_port, E_DEVICE_ADI); + claim_port_i(smart_port - 1, E_DEVICE_ADI); transform_adi_port(adi_port); - validate_type(device, adi_port, smart_port, E_ADI_DIGITAL_OUT); - if(buffer == NULL || pixel_position < 0 || buffer_length >= MAX_LED || buffer_length < 1 || pixel_position > buffer_length - 1) { + validate_type(device, adi_port, smart_port - 1, E_ADI_DIGITAL_OUT); + if (buffer == NULL || pixel_position < 0 || buffer_length >= MAX_LED || buffer_length < 1 || + pixel_position > buffer_length - 1) { errno = EINVAL; - return_port(smart_port, PROS_ERR); + return_port(smart_port - 1, PROS_ERR); } buffer[pixel_position] = color; uint32_t rtv = (uint32_t)vexDeviceAdiAddrLedSet(device->device_info, adi_port, buffer, 0, buffer_length, 0); - return_port(smart_port - 1, rtv); + return_port(smart_port - 1, rtv); } int32_t ext_adi_led_set_all(ext_adi_led_t led, uint32_t* buffer, uint32_t buffer_length, uint32_t color) { - for(int i = 0; i < buffer_length; i++){ + for (int i = 0; i < buffer_length; i++) { buffer[i] = color; - } + } return ext_adi_led_set(led, buffer, buffer_length); } diff --git a/src/devices/vdml_gps.c b/src/devices/vdml_gps.c index bdc12ce3f..4dafa3a0b 100644 --- a/src/devices/vdml_gps.c +++ b/src/devices/vdml_gps.c @@ -81,7 +81,7 @@ double gps_get_error(uint8_t port) { return_port(port - 1, rtv); } -gps_status_s_t gps_get_status(uint8_t port) { +gps_status_s_t gps_get_position_and_orientation(uint8_t port) { gps_status_s_t rtv = GPS_STATUS_ERR_INIT; if (!claim_port_try(port - 1, E_DEVICE_GPS)) { return rtv; @@ -110,34 +110,70 @@ gps_position_s_t gps_get_position(uint8_t port) { return_port(port - 1, rtv); } -double gps_get_heading(uint8_t port) { - claim_port_f(port - 1, E_DEVICE_GPS); - double rtv = vexDeviceGpsDegreesGet(device->device_info); +double gps_get_position_x(uint8_t port) { + claim_port_f(port - 1, E_DEVICE_GPS); + V5_DeviceGpsAttitude data; + vexDeviceGpsAttitudeGet(device->device_info, &data, false); + double rtv = data.position_x; return_port(port - 1, rtv); } -double gps_get_heading_raw(uint8_t port) { - claim_port_f(port - 1, E_DEVICE_GPS); - double rtv = vexDeviceGpsHeadingGet(device->device_info); +double gps_get_position_y(uint8_t port) { + claim_port_f(port - 1, E_DEVICE_GPS); + V5_DeviceGpsAttitude data; + vexDeviceGpsAttitudeGet(device->device_info, &data, false); + double rtv = data.position_y; return_port(port - 1, rtv); } -double gps_get_rotation(uint8_t port) { - claim_port_f(port - 1, E_DEVICE_GPS); - double rtv = vexDeviceGpsRotationGet(device->device_info); +gps_orientation_s_t gps_get_orientation(uint8_t port) { + gps_orientation_s_t rtv = {PROS_ERR_F, PROS_ERR_F, PROS_ERR_F}; + if (!claim_port_try(port - 1, E_DEVICE_GPS)) { + return rtv; + } + v5_smart_device_s_t* device = registry_get_device(port - 1); + V5_DeviceGpsAttitude data; + vexDeviceGpsAttitudeGet(device->device_info, &data, false); + rtv.pitch = data.pitch; + rtv.roll = data.roll; + rtv.yaw = data.yaw; return_port(port - 1, rtv); } -int32_t gps_set_rotation(uint8_t port, double target) { - claim_port_i(port - 1, E_DEVICE_GPS); - vexDeviceGpsRotationSet(device->device_info, target); - return_port(port - 1, PROS_SUCCESS); +double gps_get_pitch(uint8_t port) { + claim_port_f(port - 1, E_DEVICE_GPS); + V5_DeviceGpsAttitude data; + vexDeviceGpsAttitudeGet(device->device_info, &data, false); + double rtv = data.pitch; + return_port(port - 1, rtv); } -int32_t gps_tare_rotation(uint8_t port) { - claim_port_i(port - 1, E_DEVICE_GPS); - vexDeviceGpsRotationSet(device->device_info, 0); - return_port(port - 1, PROS_SUCCESS); +double gps_get_roll(uint8_t port) { + claim_port_f(port - 1, E_DEVICE_GPS); + V5_DeviceGpsAttitude data; + vexDeviceGpsAttitudeGet(device->device_info, &data, false); + double rtv = data.roll; + return_port(port - 1, rtv); +} + +double gps_get_yaw(uint8_t port) { + claim_port_f(port - 1, E_DEVICE_GPS); + V5_DeviceGpsAttitude data; + vexDeviceGpsAttitudeGet(device->device_info, &data, false); + double rtv = data.yaw; + return_port(port - 1, rtv); +} + +double gps_get_heading(uint8_t port) { + claim_port_f(port - 1, E_DEVICE_GPS); + double rtv = vexDeviceGpsDegreesGet(device->device_info); + return_port(port - 1, rtv); +} + +double gps_get_heading_raw(uint8_t port) { + claim_port_f(port - 1, E_DEVICE_GPS); + double rtv = vexDeviceGpsHeadingGet(device->device_info); + return_port(port - 1, rtv); } gps_gyro_s_t gps_get_gyro_rate(uint8_t port) { @@ -154,6 +190,30 @@ gps_gyro_s_t gps_get_gyro_rate(uint8_t port) { return_port(port - 1, rtv); } +double gps_get_gyro_rate_x(uint8_t port) { + claim_port_f(port - 1, E_DEVICE_GPS); + V5_DeviceGpsRaw data; + vexDeviceGpsRawGyroGet(device->device_info, &data); + double rtv = data.x; + return_port(port - 1, rtv); +} + +double gps_get_gyro_rate_y(uint8_t port) { + claim_port_f(port - 1, E_DEVICE_GPS); + V5_DeviceGpsRaw data; + vexDeviceGpsRawGyroGet(device->device_info, &data); + double rtv = data.y; + return_port(port - 1, rtv); +} + +double gps_get_gyro_rate_z(uint8_t port) { + claim_port_f(port - 1, E_DEVICE_GPS); + V5_DeviceGpsRaw data; + vexDeviceGpsRawGyroGet(device->device_info, &data); + double rtv = data.z; + return_port(port - 1, rtv); +} + gps_accel_s_t gps_get_accel(uint8_t port) { gps_accel_s_t rtv = GPS_RAW_ERR_INIT; if (!claim_port_try(port - 1, E_DEVICE_GPS)) { @@ -167,3 +227,27 @@ gps_accel_s_t gps_get_accel(uint8_t port) { rtv.z = data.z; return_port(port - 1, rtv); } + +double gps_get_accel_x(uint8_t port) { + claim_port_f(port - 1, E_DEVICE_GPS); + V5_DeviceGpsRaw data; + vexDeviceGpsRawAccelGet(device->device_info, &data); + double rtv = data.x; + return_port(port - 1, rtv); +} + +double gps_get_accel_y(uint8_t port) { + claim_port_f(port - 1, E_DEVICE_GPS); + V5_DeviceGpsRaw data; + vexDeviceGpsRawAccelGet(device->device_info, &data); + double rtv = data.y; + return_port(port - 1, rtv); +} + +double gps_get_accel_z(uint8_t port) { + claim_port_f(port - 1, E_DEVICE_GPS); + V5_DeviceGpsRaw data; + vexDeviceGpsRawAccelGet(device->device_info, &data); + double rtv = data.z; + return_port(port - 1, rtv); +} diff --git a/src/devices/vdml_gps.cpp b/src/devices/vdml_gps.cpp index 76e59f04e..4c0817c83 100644 --- a/src/devices/vdml_gps.cpp +++ b/src/devices/vdml_gps.cpp @@ -10,6 +10,7 @@ * file, You can obtain one at http://mozilla.org/MPL/2.0/. */ +#include "pros/device.h" #include "pros/gps.hpp" #include "vdml/vdml.h" @@ -37,18 +38,52 @@ std::int32_t Gps::set_data_rate(std::uint32_t rate) const { return pros::c::gps_set_data_rate(_port, rate); } +std::vector Gps::get_all_devices() { + std::vector matching_devices {Device::get_all_devices(DeviceType::gps)}; + std::vector return_vector; + for (auto device : matching_devices) { + return_vector.push_back(device); + } + return return_vector; +} + + double Gps::get_error() const { return pros::c::gps_get_error(_port); } -pros::gps_status_s_t Gps::get_status() const { - return pros::c::gps_get_status(_port); +pros::gps_status_s_t Gps::get_position_and_orientation() const { + return pros::c::gps_get_position_and_orientation(_port); } pros::gps_position_s_t Gps::get_position() const { return pros::c::gps_get_position(_port); } +double Gps::get_position_x() const { + return pros::c::gps_get_position_x(_port); +} + +double Gps::get_position_y() const { + return pros::c::gps_get_position_y(_port); +} + +pros::gps_orientation_s_t Gps::get_orientation() const { + return pros::c::gps_get_orientation(_port); +} + +double Gps::get_pitch() const { + return pros::c::gps_get_pitch(_port); +} + +double Gps::get_roll() const { + return pros::c::gps_get_roll(_port); +} + +double Gps::get_yaw() const { + return pros::c::gps_get_yaw(_port); +} + double Gps::get_heading() const { return pros::c::gps_get_heading(_port); } @@ -57,42 +92,68 @@ double Gps::get_heading_raw() const { return pros::c::gps_get_heading_raw(_port); } -double Gps::get_rotation() const { - return pros::c::gps_get_rotation(_port); +pros::gps_gyro_s_t Gps::get_gyro_rate() const { + return pros::c::gps_get_gyro_rate(_port); } -std::int32_t Gps::set_rotation(double target) const { - return pros::c::gps_set_rotation(_port, target); +double Gps::get_gyro_rate_x() const { + return pros::c::gps_get_gyro_rate_x(_port); } -std::int32_t Gps::tare_rotation() const { - return pros::c::gps_tare_rotation(_port); +double Gps::get_gyro_rate_y() const { + return pros::c::gps_get_gyro_rate_y(_port); } -pros::gps_gyro_s_t Gps::get_gyro_rate() const { - return pros::c::gps_get_gyro_rate(_port); +double Gps::get_gyro_rate_z() const { + return pros::c::gps_get_gyro_rate_z(_port); } pros::gps_accel_s_t Gps::get_accel() const { return pros::c::gps_get_accel(_port); } +double Gps::get_accel_x() const { + return pros::c::gps_get_accel_x(_port); +} + +double Gps::get_accel_y() const { + return pros::c::gps_get_accel_y(_port); +} + +double Gps::get_accel_z() const { + return pros::c::gps_get_accel_z(_port); +} + std::ostream& operator<<(std::ostream& os, const pros::Gps& gps) { - pros::gps_status_s_t data = gps.get_status(); + pros::gps_status_s_t data = gps.get_position_and_orientation(); os << "Gps ["; os << "port: " << gps._port; os << ", x: " << data.x; os << ", y: " << data.y; os << ", heading: " << gps.get_heading(); - os << ", rotation: " << gps.get_rotation(); os << "]"; return os; } +pros::Gps pros::Gps::get_gps() { + static int curr_gps_port = 0; + curr_gps_port = curr_gps_port % 21; + for (int i = 0; i < 21; i++) { + if (registry_get_device(curr_gps_port)->device_type == pros::c::E_DEVICE_GPS) { + curr_gps_port++; + return Gps(curr_gps_port); + } + curr_gps_port++; + curr_gps_port = curr_gps_port % 21; + } + errno = ENODEV; + return Gps(PROS_ERR_BYTE); +} + namespace literals { const pros::Gps operator""_gps(const unsigned long long int g) { return pros::Gps(g); } } // namespace literals -} // namespace v5 +} // namespace v5 } // namespace pros diff --git a/src/devices/vdml_imu.c b/src/devices/vdml_imu.c index b4cf27a9f..f2cd73525 100644 --- a/src/devices/vdml_imu.c +++ b/src/devices/vdml_imu.c @@ -11,6 +11,7 @@ */ #include + #include "pros/imu.h" #include "v5_api.h" #include "vdml/registry.h" @@ -28,9 +29,9 @@ } #define IMU_RESET_FLAG_SET_TIMEOUT 1000 -#define IMU_RESET_TIMEOUT 3000 // Canonically this should be 2s, but 3s for good margin +#define IMU_RESET_TIMEOUT 3000 // Canonically this should be 2s, but 3s for good margin -typedef struct __attribute__ ((packed)) imu_reset_data { +typedef struct __attribute__((packed)) imu_reset_data { double heading_offset; double rotation_offset; double pitch_offset; @@ -56,8 +57,8 @@ int32_t imu_reset(uint8_t port) { errno = EAGAIN; return PROS_ERR; } - device = device; // suppressing compiler warning - } while(!(vexDeviceImuStatusGet(device->device_info) & E_IMU_STATUS_CALIBRATING)); + device = device; // suppressing compiler warning + } while (!(vexDeviceImuStatusGet(device->device_info) & E_IMU_STATUS_CALIBRATING)); port_mutex_give(port - 1); return 1; } @@ -80,8 +81,8 @@ int32_t imu_reset_blocking(uint8_t port) { errno = EAGAIN; return PROS_ERR; } - device = device; // suppressing compiler warning - } while(!(vexDeviceImuStatusGet(device->device_info) & E_IMU_STATUS_CALIBRATING)); + device = device; // suppressing compiler warning + } while (!(vexDeviceImuStatusGet(device->device_info) & E_IMU_STATUS_CALIBRATING)); // same concept here, we add a blocking delay for the blocking version to wait // until the IMU calibrating flag is cleared do { @@ -94,8 +95,8 @@ int32_t imu_reset_blocking(uint8_t port) { errno = EAGAIN; return PROS_ERR; } - device = device; // suppressing compiler warning - } while(vexDeviceImuStatusGet(device->device_info) & E_IMU_STATUS_CALIBRATING); + device = device; // suppressing compiler warning + } while (vexDeviceImuStatusGet(device->device_info) & E_IMU_STATUS_CALIBRATING); port_mutex_give(port - 1); return 1; } @@ -118,16 +119,18 @@ int32_t imu_set_data_rate(uint8_t port, uint32_t rate) { double imu_get_rotation(uint8_t port) { claim_port_f(port - 1, E_DEVICE_IMU); ERROR_IMU_STILL_CALIBRATING(port, device, PROS_ERR_F); - double rtn = vexDeviceImuHeadingGet(device->device_info) + ((imu_data_s_t*)registry_get_device(port - 1)->pad)->rotation_offset; + double rtn = vexDeviceImuHeadingGet(device->device_info) + + ((imu_data_s_t*)registry_get_device(port - 1)->pad)->rotation_offset; return_port(port - 1, rtn); } double imu_get_heading(uint8_t port) { claim_port_f(port - 1, E_DEVICE_IMU); ERROR_IMU_STILL_CALIBRATING(port, device, PROS_ERR_F); - double rtn = vexDeviceImuDegreesGet(device->device_info) + ((imu_data_s_t*)registry_get_device(port - 1)->pad)->heading_offset; + double rtn = + vexDeviceImuDegreesGet(device->device_info) + ((imu_data_s_t*)registry_get_device(port - 1)->pad)->heading_offset; // Restricting value to raw boundaries - return_port(port - 1, fmod((rtn + IMU_HEADING_MAX), (double) IMU_HEADING_MAX)); + return_port(port - 1, fmod((rtn + IMU_HEADING_MAX), (double)IMU_HEADING_MAX)); } #define QUATERNION_ERR_INIT \ @@ -154,7 +157,7 @@ quaternion_s_t imu_get_quaternion(uint8_t port) { double cp = cos(DEGTORAD * pitch * 0.5); double sp = sin(DEGTORAD * pitch * 0.5); double cr = cos(DEGTORAD * roll * 0.5); - double sr = sin(DEGTORAD * roll * 0.5); + double sr = sin(DEGTORAD * roll * 0.5); rtn.w = cr * cp * cy + sr * sp * sy; rtn.x = sr * cp * cy - cr * sp * sy; @@ -270,9 +273,9 @@ imu_status_e_t imu_get_status(uint8_t port) { return_port(port - 1, rtn); } -//Reset Functions: -int32_t imu_tare(uint8_t port){ - if (!claim_port_try(port - 1, E_DEVICE_IMU)) { +// Reset Functions: +int32_t imu_tare(uint8_t port) { + if (!claim_port_try(port - 1, E_DEVICE_IMU)) { return PROS_ERR; } v5_smart_device_s_t* device = registry_get_device(port - 1); @@ -287,33 +290,33 @@ int32_t imu_tare(uint8_t port){ return_port(port - 1, PROS_SUCCESS); } -int32_t imu_tare_euler(uint8_t port){ - return imu_set_euler(port, (euler_s_t){0,0,0}); +int32_t imu_tare_euler(uint8_t port) { + return imu_set_euler(port, (euler_s_t){0, 0, 0}); } -int32_t imu_tare_heading(uint8_t port){ - return imu_set_heading(port, 0); +int32_t imu_tare_heading(uint8_t port) { + return imu_set_heading(port, 0); } -int32_t imu_tare_rotation(uint8_t port){ - return imu_set_rotation(port, 0); +int32_t imu_tare_rotation(uint8_t port) { + return imu_set_rotation(port, 0); } -int32_t imu_tare_pitch(uint8_t port){ - return imu_set_pitch(port, 0); +int32_t imu_tare_pitch(uint8_t port) { + return imu_set_pitch(port, 0); } -int32_t imu_tare_roll(uint8_t port){ - return imu_set_roll(port, 0); +int32_t imu_tare_roll(uint8_t port) { + return imu_set_roll(port, 0); } -int32_t imu_tare_yaw(uint8_t port){ - return imu_set_yaw(port, 0); +int32_t imu_tare_yaw(uint8_t port) { + return imu_set_yaw(port, 0); } -//Setter Functions: -int32_t imu_set_rotation(uint8_t port, double target){ - if (!claim_port_try(port - 1, E_DEVICE_IMU)) { +// Setter Functions: +int32_t imu_set_rotation(uint8_t port, double target) { + if (!claim_port_try(port - 1, E_DEVICE_IMU)) { return PROS_ERR; } v5_smart_device_s_t* device = registry_get_device(port - 1); @@ -325,8 +328,8 @@ int32_t imu_set_rotation(uint8_t port, double target){ return_port(port - 1, PROS_SUCCESS); } -int32_t imu_set_heading(uint8_t port, double target){ - if (!claim_port_try(port - 1, E_DEVICE_IMU)) { +int32_t imu_set_heading(uint8_t port, double target) { + if (!claim_port_try(port - 1, E_DEVICE_IMU)) { return PROS_ERR; } v5_smart_device_s_t* device = registry_get_device(port - 1); @@ -340,8 +343,8 @@ int32_t imu_set_heading(uint8_t port, double target){ return_port(port - 1, PROS_SUCCESS); } -int32_t imu_set_pitch(uint8_t port, double target){ - if (!claim_port_try(port - 1, E_DEVICE_IMU)) { +int32_t imu_set_pitch(uint8_t port, double target) { + if (!claim_port_try(port - 1, E_DEVICE_IMU)) { return PROS_ERR; } v5_smart_device_s_t* device = registry_get_device(port - 1); @@ -355,8 +358,8 @@ int32_t imu_set_pitch(uint8_t port, double target){ return_port(port - 1, PROS_SUCCESS); } -int32_t imu_set_roll(uint8_t port, double target){ - if (!claim_port_try(port - 1, E_DEVICE_IMU)) { +int32_t imu_set_roll(uint8_t port, double target) { + if (!claim_port_try(port - 1, E_DEVICE_IMU)) { return PROS_ERR; } v5_smart_device_s_t* device = registry_get_device(port - 1); @@ -370,8 +373,8 @@ int32_t imu_set_roll(uint8_t port, double target){ return_port(port - 1, PROS_SUCCESS); } -int32_t imu_set_yaw(uint8_t port, double target){ - if (!claim_port_try(port - 1, E_DEVICE_IMU)) { +int32_t imu_set_yaw(uint8_t port, double target) { + if (!claim_port_try(port - 1, E_DEVICE_IMU)) { return PROS_ERR; } v5_smart_device_s_t* device = registry_get_device(port - 1); @@ -385,7 +388,7 @@ int32_t imu_set_yaw(uint8_t port, double target){ return_port(port - 1, PROS_SUCCESS); } -int32_t imu_set_euler(uint8_t port, euler_s_t target){ +int32_t imu_set_euler(uint8_t port, euler_s_t target) { if (!claim_port_try(port - 1, E_DEVICE_IMU)) { return PROS_ERR; } @@ -404,3 +407,11 @@ int32_t imu_set_euler(uint8_t port, euler_s_t target){ data->yaw_offset = target.yaw - euler_values.yaw; return_port(port - 1, PROS_SUCCESS); } + +imu_orientation_e_t imu_get_physical_orientation(uint8_t port) { + imu_status_e_t status = imu_get_status(port); + if (status == E_IMU_STATUS_ERROR) { + return E_IMU_ORIENTATION_ERROR; + } + return (status >> 1) & 7; +} diff --git a/src/devices/vdml_imu.cpp b/src/devices/vdml_imu.cpp index 7fb562189..7d15169d7 100644 --- a/src/devices/vdml_imu.cpp +++ b/src/devices/vdml_imu.cpp @@ -10,6 +10,8 @@ * file, You can obtain one at http://mozilla.org/MPL/2.0/. */ +#include + #include "pros/imu.hpp" #include "vdml/vdml.h" @@ -24,6 +26,18 @@ std::int32_t Imu::set_data_rate(std::uint32_t rate) const { return pros::c::imu_set_data_rate(_port, rate); } +std::vector Imu::get_all_devices() { + + + std::vector matching_devices {Device::get_all_devices(DeviceType::imu)}; + + std::vector return_vector; + for (auto device : matching_devices) { + return_vector.push_back(device); + } + return return_vector; +} + double Imu::get_rotation() const { return pros::c::imu_get_rotation(_port); } @@ -65,7 +79,11 @@ pros::ImuStatus Imu::get_status() const { } bool Imu::is_calibrating() const { - return (int)get_status() & (int)(pros::ImuStatus::calibrating); + imu_status_e_t status = pros::c::imu_get_status(_port); + if (status == E_IMU_STATUS_ERROR) { + return false; + } + return status & E_IMU_STATUS_CALIBRATING; } std::int32_t Imu::tare_heading() const { @@ -120,6 +138,25 @@ std::int32_t Imu::tare() const { return pros::c::imu_tare(_port); } +imu_orientation_e_t Imu::get_physical_orientation() const { + return pros::c::imu_get_physical_orientation(_port); +} + +Imu Imu::get_imu() { + static int curr_imu_port = 0; + curr_imu_port = curr_imu_port % 21; + for (int i = 0; i < 21; i++) { + if (registry_get_device(curr_imu_port)->device_type == pros::c::E_DEVICE_IMU) { + curr_imu_port++; + return Imu(curr_imu_port); + } + curr_imu_port++; + curr_imu_port = curr_imu_port % 21; + } + errno = ENODEV; + return Imu(PROS_ERR_BYTE); +} + std::ostream& operator<<(std::ostream& os, const pros::Imu& imu) { pros::imu_gyro_s_t gyro = imu.get_gyro_rate(); pros::imu_accel_s_t accel = imu.get_accel(); diff --git a/src/devices/vdml_motorgroup.cpp b/src/devices/vdml_motorgroup.cpp index ca9b37cfb..ab63102e2 100644 --- a/src/devices/vdml_motorgroup.cpp +++ b/src/devices/vdml_motorgroup.cpp @@ -11,6 +11,7 @@ */ #include "kapi.h" +#include "pros/abstract_motor.hpp" #include "pros/motor_group.hpp" #include "pros/motors.hpp" @@ -31,36 +32,33 @@ using namespace pros::c; #define empty_MotorGroup_check_vector(error, vector) \ if (_ports.size() <= 0) { \ errno = EDOM; \ - vector.push_back(error); \ + vector.push_back(error); \ return vector; \ } -MotorGroup::MotorGroup(AbstractMotor& abstract_motor) : MotorGroup(abstract_motor.get_port_all()) { -} +MotorGroup::MotorGroup(AbstractMotor& motor_group) : MotorGroup(motor_group.get_port_all()) {} -MotorGroup::MotorGroup(const std::initializer_list ports, - const pros::v5::MotorGears gearset, +MotorGroup::MotorGroup(const std::initializer_list ports, const pros::v5::MotorGears gearset, const pros::v5::MotorUnits encoder_units) : _ports(ports) { - set_gearing(gearset); - set_encoder_units(encoder_units); + if (gearset != pros::v5::MotorGears::invalid) { + set_gearing_all(gearset); + } + if (encoder_units != pros::v5::MotorUnits::invalid) { + set_encoder_units_all(encoder_units); + } } -MotorGroup::MotorGroup(const std::vector& ports, - const pros::v5::MotorGears gearset, +MotorGroup::MotorGroup(const std::vector& ports, const pros::v5::MotorGears gearset, const pros::v5::MotorUnits encoder_units) : _ports(ports) { - set_gearing(gearset); - set_encoder_units(encoder_units); -} - -std::int32_t MotorGroup::operator=(std::int32_t voltage) const { - empty_MotorGroup_check(PROS_ERR); - for (auto it = _ports.begin() + 1; it < _ports.end(); it++) { - motor_move(*it, voltage); + if (gearset != pros::v5::MotorGears::invalid) { + set_gearing_all(gearset); + } + if (encoder_units != pros::v5::MotorUnits::invalid) { + set_encoder_units_all(encoder_units); } - return motor_move(_ports[0], voltage); } std::int32_t MotorGroup::move(std::int32_t voltage) const { @@ -561,7 +559,27 @@ std::int32_t MotorGroup::set_gearing(const motor_gearset_e_t gearset, const std: MotorGroup_index_check(PROS_ERR, index); return motor_set_gearing(_ports[index], gearset); } +std::int32_t MotorGroup::set_gearing(std::vector gearsets) const { + empty_MotorGroup_check(PROS_ERR); + for (int i = 0; i < gearsets.size(); i++) { + this->set_gearing(gearsets[i], _ports[i]); + } + if (gearsets.size() != _ports.size()) { + errno = E2BIG; + } + return PROS_SUCCESS; +} +std::int32_t MotorGroup::set_gearing(std::vector gearsets) const { + empty_MotorGroup_check(PROS_ERR); + for (int i = 0; i < gearsets.size(); i++) { + this->set_gearing(gearsets[i], _ports[i]); + } + if (gearsets.size() != _ports.size()) { + errno = E2BIG; + } + return PROS_SUCCESS; +} std::int32_t MotorGroup::set_gearing(const pros::v5::MotorGear gearset, const std::uint8_t index) const { empty_MotorGroup_check(PROS_ERR); MotorGroup_index_check(PROS_ERR, index); @@ -636,13 +654,15 @@ std::int8_t MotorGroup::size() const { return _ports.size(); } -void MotorGroup::operator+=(MotorGroup& other) { - for (auto it = other._ports.begin(); it < other._ports.end(); it++) { - _ports.push_back(*it); +void MotorGroup::operator+=(AbstractMotor& other) { + auto ports = other.get_port_all(); + for (auto it = ports.begin(); it < ports.end(); it++) { + auto port = *it; + _ports.push_back(port); } } -void MotorGroup::append(MotorGroup& other) { +void MotorGroup::append(AbstractMotor& other) { (*this) += other; } diff --git a/src/devices/vdml_motors.c b/src/devices/vdml_motors.c index 63b2a0fde..ea0720bde 100644 --- a/src/devices/vdml_motors.c +++ b/src/devices/vdml_motors.c @@ -308,5 +308,5 @@ int32_t motor_get_voltage_limit(int8_t port) { port = abs(port); claim_port_i(port - 1, E_DEVICE_MOTOR); int32_t rtn = vexDeviceMotorVoltageLimitGet(device->device_info); - return_port(rtn, port - 1); + return_port(port - 1, rtn); } diff --git a/src/devices/vdml_motors.cpp b/src/devices/vdml_motors.cpp index fae02581e..079b391a1 100644 --- a/src/devices/vdml_motors.cpp +++ b/src/devices/vdml_motors.cpp @@ -18,18 +18,15 @@ namespace pros { inline namespace v5 { using namespace pros::c; -Motor::Motor(AbstractMotor& abstract_motor) : Motor(abstract_motor.get_port()) { - -} Motor::Motor(const std::int8_t port, const pros::v5::MotorGears gearset, const pros::v5::MotorUnits encoder_units) : Device(port, DeviceType::motor), _port(port) { - set_gearing(gearset); - set_encoder_units(encoder_units); -} - -std::int32_t Motor::operator=(std::int32_t voltage) const { - return motor_move(_port, voltage); + if (gearset != pros::v5::MotorGears::invalid) { + set_gearing(gearset); + } + if (encoder_units != pros::v5::MotorEncoderUnits::invalid) { + set_encoder_units(encoder_units); + } } std::int32_t Motor::move(std::int32_t voltage) const { @@ -354,11 +351,22 @@ std::int32_t Motor::get_voltage_limit(const std::uint8_t index) const { } return motor_get_voltage_limit(_port); } + std::vector Motor::get_voltage_limit_all(void) const { std::vector return_vector; return_vector.push_back(motor_get_voltage_limit(_port)); return return_vector; } + +std::vector Motor::get_all_devices() { + std::vector matching_devices {Device::get_all_devices(DeviceType::motor)}; + std::vector return_vector; + for (auto device : matching_devices) { + return_vector.push_back(device); + } + return return_vector; +} + std::int8_t Motor::get_port(const std::uint8_t index) const { if (index != 0) { errno = EOVERFLOW; diff --git a/src/devices/vdml_optical.cpp b/src/devices/vdml_optical.cpp index e6b3dd182..78e448f1d 100644 --- a/src/devices/vdml_optical.cpp +++ b/src/devices/vdml_optical.cpp @@ -19,6 +19,15 @@ using namespace pros::c; Optical::Optical(std::uint8_t port) : Device(port, DeviceType::optical) {} +std::vector Optical::get_all_devices() { + std::vector matching_devices {Device::get_all_devices(DeviceType::optical)}; + std::vector return_vector; + for (auto device : matching_devices) { + return_vector.push_back(device); + } + return return_vector; +} + double Optical::get_hue(){ return optical_get_hue(_port); } diff --git a/src/devices/vdml_rotation.cpp b/src/devices/vdml_rotation.cpp index c937dc03a..cbfafab6f 100644 --- a/src/devices/vdml_rotation.cpp +++ b/src/devices/vdml_rotation.cpp @@ -15,13 +15,13 @@ namespace pros { inline namespace v5 { - -Rotation::Rotation(const std::int8_t port) : Device(port, DeviceType::rotation) { - if (port < 0) { - pros::c::rotation_set_reversed(abs(port), true); - } else { - pros::c::rotation_set_reversed(port, false); - } + +Rotation::Rotation(const std::int8_t port) : Device(abs(port), DeviceType::rotation) { + if (port < 0) { + pros::c::rotation_set_reversed(abs(port), true); + } else { + pros::c::rotation_set_reversed(port, false); + } } std::int32_t Rotation::reset() { @@ -40,6 +40,15 @@ std::int32_t Rotation::reset_position(void) const { return pros::c::rotation_reset_position(_port); } +std::vector Rotation::get_all_devices() { + std::vector matching_devices {Device::get_all_devices(DeviceType::rotation)}; + std::vector return_vector; + for (auto device : matching_devices) { + return_vector.push_back(device); + } + return return_vector; +} + std::int32_t Rotation::get_position(void) const { return pros::c::rotation_get_position(_port); } @@ -77,9 +86,9 @@ std::ostream& operator<<(std::ostream& os, const pros::Rotation& rotation) { namespace literals { const pros::Rotation operator"" _rot(const unsigned long long int r) { - return pros::Rotation(r); + return pros::Rotation(r); } -} // namespace literals +} // namespace literals } // namespace v5 } // namespace pros diff --git a/src/devices/vdml_usd.c b/src/devices/vdml_usd.c index 879f6525f..da60f3f93 100644 --- a/src/devices/vdml_usd.c +++ b/src/devices/vdml_usd.c @@ -17,3 +17,15 @@ int32_t usd_is_installed(void) { return vexFileDriveStatus(0); } +static const int FRESULTMAP[] = {0, EIO, EINVAL, EBUSY, ENOENT, ENOENT, EINVAL, EACCES, // FR_DENIED + EEXIST, EINVAL, EROFS, ENXIO, ENOBUFS, ENXIO, EIO, EACCES, // FR_LOCKED + ENOBUFS, ENFILE, EINVAL}; + +int32_t usd_list_files(const char* path, char* buffer, int32_t len) { + FRESULT result = vexFileDirectoryGet(path, buffer, len); + if (result != F_OK) { + errno = FRESULTMAP[result]; + return PROS_ERR; + } + return PROS_SUCCESS; +} diff --git a/src/devices/vdml_usd.cpp b/src/devices/vdml_usd.cpp index 71318463c..d6ede6edd 100644 --- a/src/devices/vdml_usd.cpp +++ b/src/devices/vdml_usd.cpp @@ -21,5 +21,9 @@ std::int32_t is_installed(void) { return usd_is_installed(); } +int32_t list_files(const char* path, char* buffer, int32_t len) { + return usd_list_files(path, buffer, len); +} + } // namespace usd } // namespace pros diff --git a/src/devices/vdml_vision.cpp b/src/devices/vdml_vision.cpp index 9349d4396..162a18d35 100644 --- a/src/devices/vdml_vision.cpp +++ b/src/devices/vdml_vision.cpp @@ -39,6 +39,17 @@ vision_color_code_t Vision::create_color_code(const std::uint32_t sig_id1, const return vision_create_color_code(_port, sig_id1, sig_id2, sig_id3, sig_id4, sig_id5); } +std::vector Vision::get_all_devices() { + + std::vector matching_devices{Device::get_all_devices(DeviceType::vision)}; + + std::vector return_vector; + for (auto device : matching_devices) { + return_vector.push_back(device); + } + return return_vector; +} + vision_object_s_t Vision::get_by_size(const std::uint32_t size_id) const { return vision_get_by_size(_port, size_id); } @@ -63,7 +74,6 @@ std::int32_t Vision::get_white_balance(void) const { return vision_get_white_balance(_port); } - int32_t Vision::read_by_size(const std::uint32_t size_id, const std::uint32_t object_count, vision_object_s_t* const object_arr) const { return vision_read_by_size(_port, size_id, object_count, object_arr); @@ -114,6 +124,26 @@ std::int32_t Vision::set_zero_point(vision_zero_e_t zero_point) const { std::int32_t Vision::set_wifi_mode(const std::uint8_t enable) const { return vision_set_wifi_mode(_port, enable); } +Vision Vision::get_vision() { + static int curr_vision_port = 0; + curr_vision_port = curr_vision_port % 21; + for (int i = 0; i < 21; i++) { + if (registry_get_device(curr_vision_port)->device_type == pros::c::E_DEVICE_VISION) { + curr_vision_port++; + return Vision(curr_vision_port); + } + curr_vision_port++; + curr_vision_port = curr_vision_port % 21; + } + errno = ENODEV; + return Vision(PROS_ERR_BYTE); +} } // namespace v5 +namespace literals { +const pros::Vision operator"" _vis(const unsigned long long int m) { + return Vision(m); +} + +} // namespace literals } // namespace pros diff --git a/src/main.cpp b/src/main.cpp index f384cc4f3..7fb873947 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -75,19 +75,20 @@ void autonomous() {} */ 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 + 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 5 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::lcd::read_buttons() & LCD_BTN_RIGHT) >> 0); // Prints status of the emulated screen LCDs + // 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 = dir - turn; // Sets left motor voltage - right_mg = dir + turn; // Sets right motor voltage - pros::delay(20); // Run for 20 ms then update + 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 } } \ No newline at end of file diff --git a/src/system/dev/usd_driver.c b/src/system/dev/usd_driver.c index 8c703b1d7..50bd39365 100644 --- a/src/system/dev/usd_driver.c +++ b/src/system/dev/usd_driver.c @@ -54,6 +54,8 @@ int usd_write_r(struct _reent* r, void* const arg, const uint8_t* buf, const siz usd_file_arg_t* file_arg = (usd_file_arg_t*)arg; // TODO: mutex here. Global or file lock? int32_t result = vexFileWrite((char*)buf, sizeof(*buf), len, file_arg->ifi_fptr); + // Flush the buffer + vexFileSync(file_arg->ifi_fptr); return result; } diff --git a/src/system/dev/vfs.c b/src/system/dev/vfs.c index c6d0d750b..6f090f6c0 100644 --- a/src/system/dev/vfs.c +++ b/src/system/dev/vfs.c @@ -109,10 +109,9 @@ int _open(const char* file, int flags, int mode) { return usd_open_r(r, file + strlen("/usd"), flags, mode); } else if (strstr(file, "/dev") == file) { return dev_open_r(r, file + strlen("/dev"), flags, mode); + } else { + return usd_open_r(r, file, flags, mode); } - - r->_errno = ENOENT; - return -1; } ssize_t _write(int file, const void* buf, size_t len) { diff --git a/version b/version index d13e837c8..ee74734aa 100644 --- a/version +++ b/version @@ -1 +1 @@ -4.0.6 +4.1.0