diff --git a/firmware/libv5rts b/firmware/libv5rts index e2a6f9e2f..6113a67b5 160000 --- a/firmware/libv5rts +++ b/firmware/libv5rts @@ -1 +1 @@ -Subproject commit e2a6f9e2f7488f7352b90d6a95a32c866702b5fa +Subproject commit 6113a67b5599301020a8bf81b7a0456cb298d703 diff --git a/include/api.h b/include/api.h index 15ae25d2c..c682dc1d7 100644 --- a/include/api.h +++ b/include/api.h @@ -51,6 +51,7 @@ #include "pros/colors.h" #include "pros/distance.h" #include "pros/ext_adi.h" +#include "pros/gps.h" #include "pros/imu.h" #include "pros/llemu.h" #include "pros/misc.h" @@ -64,6 +65,7 @@ #ifdef __cplusplus #include "pros/adi.hpp" #include "pros/distance.hpp" +#include "pros/gps.hpp" #include "pros/imu.hpp" #include "pros/llemu.hpp" #include "pros/misc.hpp" diff --git a/include/pros/apix.h b/include/pros/apix.h index f95e7d075..14c8a3138 100644 --- a/include/pros/apix.h +++ b/include/pros/apix.h @@ -376,6 +376,7 @@ typedef enum v5_device_e { E_DEVICE_VISION = 11, E_DEVICE_ADI = 12, E_DEVICE_OPTICAL = 16, + E_DEVICE_GPS = 20, E_DEVICE_GENERIC = 129, E_DEVICE_UNDEFINED = 255 } v5_device_e_t; diff --git a/include/pros/gps.h b/include/pros/gps.h new file mode 100644 index 000000000..0a83edea9 --- /dev/null +++ b/include/pros/gps.h @@ -0,0 +1,313 @@ +/** + * \file pros/gps.h + * + * Contains prototypes for functions related to the VEX GPS. + * + * Visit https://pros.cs.purdue.edu/v5/api/c/gps.html to learn + * more. + * + * This file should not be modified by users, since it gets replaced whenever + * a kernel upgrade occurs. + * + * Copyright (c) 2017-2021, Purdue University ACM SIGBots. + * + * 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/. + */ + +#ifndef _PROS_GPS_H_ +#define _PROS_GPS_H_ + +#include +#include + +#ifdef __cplusplus +extern "C" { +namespace pros { +namespace c { +#endif + +typedef struct __attribute__((__packed__)) gps_status_s { + double x; ///< X Position (meters) + double y; ///< Y Position (meters) + double pitch; ///< Percieved Pitch based on GPS + IMU + double roll; ///< Percieved Roll based on GPS + IMU + double yaw; ///< Percieved Yaw based on GPS + IMU +} gps_status_s_t; + +struct gps_raw_s { + double x; ///< Percieved Pitch based on GPS + IMU + double y; ///< Percieved Roll based on GPS + IMU + double z; ///< Percieved Yaw based on GPS + IMU +}; + +typedef struct gps_raw_s gps_accel_s_t; +typedef struct gps_raw_s gps_gyro_s_t; + +/** + * Set the GPS's offset relative to the center of turning in meters, + * as well as its initial 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 + * + * \param port + * The V5 GPS port number from 1-21 + * \param xOffset + * Cartesian 4-Quadrant X offset from center of turning (meters) + * \param yOffset + * Cartesian 4-Quadrant Y offset from center of turning (meters) + * \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. + */ +int32_t gps_initialize_full(uint8_t port, double xInitial, double yInitial, double headingInitial, double xOffset, + double yOffset); + +/** + * Set the GPS's offset relative to the center of turning 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 + * \param xOffset + * Cartesian 4-Quadrant X offset from center of turning (meters) + * \param yOffset + * Cartesian 4-Quadrant Y offset from center of turning (meters) + * \return 1 if the operation was successful or PROS_ERR if the operation + * failed, setting errno. + */ +int32_t gps_set_offset(uint8_t port, double xOffset, double yOffset); + +/** + * Get the GPS's 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 + * \param xOffset + * Pointer to cartesian 4-Quadrant X offset from center of turning (meters) + * \param yOffset + * Pointer to cartesian 4-Quadrant Y offset from center of turning (meters) + * \return 1 if the operation was successful or PROS_ERR if the operation + * failed, setting errno. + */ +int32_t gps_get_offset(uint8_t port, double* xOffset, double* yOffset); + +/** + * Sets the robot's location relative to the center of the field 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 + * \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. + */ +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. + * + * 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. + */ +int32_t gps_set_data_rate(uint8_t port, uint32_t rate); + +/** + * 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: + * 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 Possible RMS (Root Mean Squared) error in meters for GPS position. + * If the operation failed, returns PROS_ERR_F and errno is set. + */ +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. + */ +gps_status_s_t gps_get_status(uint8_t port); + +/** + * Get the heading in [0,360) degree values. + * + * 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 heading in [0,360) degree values. If the operation failed, + * returns PROS_ERR_F and errno is set. + */ +double gps_get_heading(uint8_t port); + +/** + * Get the heading in the max double value and min double value scale. + * + * 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 heading in [DOUBLE_MIN, DOUBLE_MAX] values. If the operation + * fails, returns PROS_ERR_F and errno is set. + */ +double gps_get_heading_raw(uint8_t port); + +/** + * Gets the GPS sensor's elapsed rotation value + * + * 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 elased heading in degrees. If the operation fails, returns + * PROS_ERR_F and errno is set. + */ +double gps_get_rotation(uint8_t port); + +/** + * Set the GPS sensor's rotation value to target value + * + * 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. + */ +int32_t gps_set_rotation(uint8_t port, double target); + +/** + * Tare the GPS sensor's rotation value + * + * 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. + */ +int32_t gps_tare_rotation(uint8_t port); + +/** + * Get the GPS's raw gyroscope values + * + * 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. + */ +gps_gyro_s_t gps_get_gyro_rate(uint8_t port); + +/** + * Get the GPS's raw accelerometer values + * + * 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 values. If the operation failed, all the + * structure's members are filled with PROS_ERR_F and errno is set. + */ +gps_accel_s_t gps_get_accel(uint8_t port); + +#ifdef __cplusplus +} +} +} +#endif + +#endif diff --git a/include/pros/gps.hpp b/include/pros/gps.hpp new file mode 100644 index 000000000..fd4ed96d3 --- /dev/null +++ b/include/pros/gps.hpp @@ -0,0 +1,284 @@ +/** + * \file pros/gps.hpp + * + * Contains prototypes for functions related to the VEX GPS. + * + * Visit https://pros.cs.purdue.edu/v5/api/cpp/gps.html to learn + * more. + * + * This file should not be modified by users, since it gets replaced whenever + * a kernel upgrade occurs. + * + * Copyright (c) 2017-2021, Purdue University ACM SIGBots. + * + * 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/. + */ + +#ifndef _PROS_IMU_HPP_ +#define _PROS_IMU_HPP_ + +#include + +#include + +#include "pros/gps.h" + +namespace pros { +class Gps { + const std::uint8_t _port; + + public: + Gps(const std::uint8_t port) : _port(port){}; + + Gps(const std::uint8_t port, double xInitial, double yInitial, double headingInitial) : _port(port) { + pros::c::gps_set_position(port, xInitial, yInitial, headingInitial); + }; + + Gps(const std::uint8_t port, double xOffset, double yOffset) : _port(port) { + pros::c::gps_set_offset(port, xOffset, yOffset); + }; + + Gps(const std::uint8_t port, double xInitial, double yInitial, double headingInitial, double xOffset, double yOffset) + : _port(port) { + pros::c::gps_initialize_full(port, xInitial, yInitial, headingInitial, xOffset, yOffset); + }; + + /** + * Set the GPS's offset relative to the center of turning in meters, + * as well as its initial 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 + * + * \param xOffset + * Cartesian 4-Quadrant X offset from center of turning (meters) + * \param yOffset + * Cartesian 4-Quadrant Y offset from center of turning (meters) + * \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. + */ + virtual std::int32_t initialize_full(double xInitial, double yInitial, double headingInitial, double xOffset, + double yOffset) const; + + /** + * Set the GPS's offset relative to the center of turning 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 xOffset + * Cartesian 4-Quadrant X offset from center of turning (meters) + * \param yOffset + * Cartesian 4-Quadrant Y offset from center of turning (meters) + * \return 1 if the operation was successful or PROS_ERR if the operation + * failed, setting errno. + */ + virtual std::int32_t set_offset(double xOffset, double yOffset) const; + + /** + * Get the GPS's 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 xOffset + * Pointer to cartesian 4-Quadrant X offset from center of turning (meters) + * \param yOffset + * Pointer to cartesian 4-Quadrant Y offset from center of turning (meters) + * \return 1 if the operation was successful or PROS_ERR if the operation + * failed, setting errno. + */ + virtual std::int32_t get_offset(double* xOffset, double* yOffset) const; + + /** + * Sets the robot's location relative to the center of the field 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 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. + */ + virtual std::int32_t set_position(double xInitial, double yInitial, double headingInitial) const; + + /** + * Set the GPS sensor's data rate in milliseconds, only applies to IMU on 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 rate + * Data rate in milliseconds (Minimum: 5 ms) + * \return 1 if the operation was successful or PROS_ERR if the operation + * failed, setting errno. + */ + virtual std::int32_t set_data_rate(std::uint32_t rate) const; + + /** + * 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: + * 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 Possible RMS (Root Mean Squared) error in meters for GPS position. + * If the operation failed, returns PROS_ERR_F and errno is set. + */ + virtual double get_error() const; + + /** + * 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 + * + * + * \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. + */ + virtual pros::c::gps_status_s_t get_status() const; + + /** + * Get the heading in [0,360) degree values. + * + * 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 heading in [0,360) degree values. If the operation failed, + * returns PROS_ERR_F and errno is set. + */ + virtual double get_heading() const; + + /** + * Get the heading in the max double value and min double value scale. + * + * 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 heading in [DOUBLE_MIN, DOUBLE_MAX] values. If the operation + * fails, returns PROS_ERR_F and errno is set. + */ + virtual double get_heading_raw() const; + + /** + * Gets the GPS sensor's elapsed rotation value + * + * 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 elased heading in degrees. If the operation fails, returns + * PROS_ERR_F and errno is set. + */ + virtual double get_rotation() const; + + /** + * Set the GPS sensor's rotation value to target value + * + * 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 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. + */ + virtual std::int32_t set_rotation(double target) const; + + /** + * Tare the GPS sensor's rotation value + * + * 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 1 if the operation was successful or PROS_ERR if the operation + * failed, setting errno. + */ + virtual std::int32_t tare_rotation() const; + + /** + * Get the GPS's raw gyroscope values + * + * 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 raw gyroscope values. If the operation failed, all the + * structure's members are filled with PROS_ERR_F and errno is set. + */ + virtual pros::c::gps_gyro_s_t get_gyro_rate() const; + + /** + * Get the GPS's raw accelerometer values + * + * 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 values. If the operation failed, all the + * structure's members are filled with PROS_ERR_F and errno is set. + */ + virtual pros::c::gps_accel_s_t get_accel() const; + +}; // Gps Class + +using GPS = Gps; + +} // namespace pros +#endif diff --git a/src/devices/vdml_gps.c b/src/devices/vdml_gps.c new file mode 100644 index 000000000..fafc0e184 --- /dev/null +++ b/src/devices/vdml_gps.c @@ -0,0 +1,146 @@ +/** + * \file devices/vdml_gps.c + * + * Contains functions for interacting with the VEX GPS. + * + * Copyright (c) 2017-2021, Purdue University ACM SIGBots. + * + * 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/. + */ + +#include + +#include "pros/gps.h" +#include "v5_api.h" +#include "vdml/registry.h" +#include "vdml/vdml.h" + +#define GPS_MINIMUM_DATA_RATE 5 + +#define GPS_STATUS_ERR_INIT \ + { .x = PROS_ERR_F, .y = PROS_ERR_F, .roll = PROS_ERR_F, .pitch = PROS_ERR_F, .yaw = PROS_ERR_F } + +#define GPS_RAW_ERR_INIT \ + { .x = PROS_ERR_F, .y = PROS_ERR_F, .z = PROS_ERR_F } + +int32_t gps_initialize_full(uint8_t port, double xInitial, double yInitial, double headingInitial, double xOffset, + double yOffset) { + claim_port_i(port - 1, E_DEVICE_GPS); + vexDeviceGpsOriginSet(device->device_info, xOffset, yOffset); + vexDeviceGpsInitialPositionSet(device->device_info, xInitial, yInitial, headingInitial); + return_port(port - 1, 1); +} + +int32_t gps_set_offset(uint8_t port, double xOffset, double yOffset) { + claim_port_i(port - 1, E_DEVICE_GPS); + vexDeviceGpsOriginSet(device->device_info, xOffset, yOffset); + return_port(port - 1, 1); +} + +int32_t gps_get_offset(uint8_t port, double* xOffset, double* yOffset) { + claim_port_i(port - 1, E_DEVICE_GPS); + vexDeviceGpsOriginGet(device->device_info, xOffset, yOffset); + return_port(port - 1, 1); +} + +int32_t gps_set_position(uint8_t port, double xInitial, double yInitial, double headingInitial) { + claim_port_i(port - 1, E_DEVICE_GPS); + vexDeviceGpsInitialPositionSet(device->device_info, xInitial, yInitial, headingInitial); + return_port(port - 1, 1); +} + +int32_t gps_set_data_rate(uint8_t port, uint32_t rate) { + claim_port_i(port - 1, E_DEVICE_GPS); + + // rate is not less than 5ms, and rounded down to nearest increment of 5 + if (rate < GPS_MINIMUM_DATA_RATE) { + rate = GPS_MINIMUM_DATA_RATE; + } else { + rate -= rate % GPS_MINIMUM_DATA_RATE; + } + + vexDeviceGpsDataRateSet(device->device_info, rate); + return_port(port - 1, 1); +} + +double gps_get_error(uint8_t port) { + claim_port_f(port - 1, E_DEVICE_GPS); + double rtv = vexDeviceGpsErrorGet(device->device_info); + return_port(port - 1, rtv); +} + +gps_status_s_t gps_get_status(uint8_t port) { + gps_status_s_t rtv = GPS_STATUS_ERR_INIT; + 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.x = data.position_x; + rtv.y = data.position_y; + rtv.pitch = data.pitch; + rtv.roll = data.roll; + rtv.yaw = 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); +} + +double gps_get_rotation(uint8_t port) { + claim_port_f(port - 1, E_DEVICE_GPS); + double rtv = vexDeviceGpsRotationGet(device->device_info); + 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, 1); +} + +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, 1); +} + +gps_gyro_s_t gps_get_gyro_rate(uint8_t port) { + gps_gyro_s_t rtv = GPS_RAW_ERR_INIT; + if (!claim_port_try(port - 1, E_DEVICE_GPS)) { + return rtv; + } + v5_smart_device_s_t* device = registry_get_device(port - 1); + V5_DeviceGpsRaw data; + vexDeviceGpsRawGyroGet(device->device_info, &data); + rtv.x = data.x; + rtv.y = data.y; + rtv.z = 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)) { + return rtv; + } + v5_smart_device_s_t* device = registry_get_device(port - 1); + V5_DeviceGpsRaw data; + vexDeviceGpsRawAccelGet(device->device_info, &data); + rtv.x = data.x; + rtv.y = data.y; + rtv.z = data.z; + return_port(port - 1, rtv); +} diff --git a/src/devices/vdml_gps.cpp b/src/devices/vdml_gps.cpp new file mode 100644 index 000000000..5410efd8b --- /dev/null +++ b/src/devices/vdml_gps.cpp @@ -0,0 +1,74 @@ +/** + * \file devices/vdml_gps.cpp + * + * Contains functions for interacting with the VEX GPS. + * + * Copyright (c) 2017-2021, Purdue University ACM SIGBots. + * + * 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/. + */ + +#include "pros/gps.hpp" + +namespace pros { + +std::int32_t Gps::initialize_full(double xInitial, double yInitial, double headingInitial, double xOffset, + double yOffset) const { + return pros::c::gps_initialize_full(_port, xInitial, yInitial, headingInitial, xOffset, yOffset); +} + +std::int32_t Gps::set_offset(double xOffset, double yOffset) const { + return pros::c::gps_set_offset(_port, xOffset, yOffset); +} + +std::int32_t Gps::get_offset(double* xOffset, double* yOffset) const { + return pros::c::gps_get_offset(_port, xOffset, yOffset); +} + +std::int32_t Gps::set_position(double xInitial, double yInitial, double headingInitial) const { + return pros::c::gps_set_position(_port, xInitial, yInitial, headingInitial); +} + +std::int32_t Gps::set_data_rate(std::uint32_t rate) const { + return pros::c::gps_set_data_rate(_port, rate); +} + +double Gps::get_error() const { + return pros::c::gps_get_error(_port); +} + +pros::c::gps_status_s_t Gps::get_status() const { + return pros::c::gps_get_status(_port); +} + +double Gps::get_heading() const { + return pros::c::gps_get_heading(_port); +} + +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); +} + +std::int32_t Gps::set_rotation(double target) const { + return pros::c::gps_set_rotation(_port, target); +} + +std::int32_t Gps::tare_rotation() const { + return pros::c::gps_tare_rotation(_port); +} + +pros::c::gps_gyro_s_t Gps::get_gyro_rate() const { + return pros::c::gps_get_gyro_rate(_port); +} + +pros::c::gps_accel_s_t Gps::get_accel() const { + return pros::c::gps_get_accel(_port); +} + +} // namespace pros diff --git a/src/devices/vdml_rotation.c b/src/devices/vdml_rotation.c index 19d150fe3..a9a0027e0 100644 --- a/src/devices/vdml_rotation.c +++ b/src/devices/vdml_rotation.c @@ -24,7 +24,7 @@ int32_t rotation_reset(uint8_t port) { } int32_t rotation_set_data_rate(uint8_t port, uint32_t rate) { - claim_port_i(port - 1, E_DEVICE_IMU); + claim_port_i(port - 1, E_DEVICE_ROTATION); // rate is not less than 5ms, and rounded down to nearest increment of 5 if (rate < ROTATION_MINIMUM_DATA_RATE) { diff --git a/version b/version index 1545d9665..d5c0c9914 100644 --- a/version +++ b/version @@ -1 +1 @@ -3.5.0 +3.5.1