From e916e915952b17ee1f9b2c689607fb09193d5950 Mon Sep 17 00:00:00 2001 From: Exortions <75327059+Exortions@users.noreply.github.com> Date: Wed, 26 Feb 2025 21:34:44 -0800 Subject: [PATCH 01/11] =?UTF-8?q?=F0=9F=92=A5=20Delete=20old=20VDML=20impl?= =?UTF-8?q?ementation?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- include/vdml/port.h | 31 ----- include/vdml/registry.h | 99 --------------- include/vdml/vdml.h | 244 ------------------------------------ src/devices/vdml.c | 269 ---------------------------------------- 4 files changed, 643 deletions(-) delete mode 100644 include/vdml/port.h delete mode 100644 include/vdml/registry.h delete mode 100644 include/vdml/vdml.h delete mode 100644 src/devices/vdml.c diff --git a/include/vdml/port.h b/include/vdml/port.h deleted file mode 100644 index df93ba87..00000000 --- a/include/vdml/port.h +++ /dev/null @@ -1,31 +0,0 @@ -/** - * \file devices/port.h - * - * This file contains the standard header info for port macros and bit masks, - * used mostly for the adi expander. - * - * \copyright Copyright (c) 2017-2024, Purdue University ACM SIGBots. - * All rights reserved. - * - * 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/. - */ - -#define SMART_PORT_BITS 16 -#define SMART_PORT_MASK ((1 << SMART_PORT_BITS) - 1) - -/** - * Macro Description: Given a merged ports variable, it sets the smart port and adi port to the values inside the - * int32_t. - */ -#define get_ports(ports, smart_port, adi_port) \ - { \ - uint32_t uport = (uint32_t)ports; \ - smart_port = uport & SMART_PORT_MASK; \ - adi_port = uport >> SMART_PORT_BITS; \ - } - -static inline uint32_t merge_adi_ports(uint8_t smart_port, uint8_t adi_port) { - return (adi_port << SMART_PORT_BITS) | smart_port; -} diff --git a/include/vdml/registry.h b/include/vdml/registry.h deleted file mode 100644 index b6855ac6..00000000 --- a/include/vdml/registry.h +++ /dev/null @@ -1,99 +0,0 @@ -/** - * \file vdml/registry.h - * - * This file contains the standard header info for the VDML (Vex Data Management - * Layer) registry. - * - * \copyright Copyright (c) 2017-2024, 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/. - */ - -#pragma once - -#include "pros/apix.h" -#include "v5_api.h" -#include "vdml/vdml.h" - -#ifdef __cplusplus -#define v5_device_e_t pros::c::v5_device_e_t -extern "C" { -#endif - -typedef struct { - v5_device_e_t device_type; - V5_DeviceT device_info; - uint8_t pad[128]; // 16 bytes in adi_data_s_t times 8 ADI Ports = 128 -} v5_smart_device_s_t; - -/* - * Detects the devices that are plugged in. - * - * Pulls the type names of plugged-in devices and stores them in the buffer - * registry_types. - */ -void registry_update_types(); - -/* - * Returns the information on the device registered to the 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). - * - * \param port - * The V5 port number from 1-21 - * - * \return A struct containing the device type and the info needed for api - * functions - */ -v5_smart_device_s_t* registry_get_device(uint8_t port); - -/* - * Returns the information on the device registered to the 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). - * - * \param port - * The V5 port number from 0-32 - * - * \return A struct containing the device type and the info needed for api - * functions - */ -v5_smart_device_s_t* registry_get_device_internal(uint8_t port); - -/* - * Checks whether there is a discrepancy between the binding of the port and - * what is actually plugged in. - * - * If a device is plugged in but not registered, registers the port. - * If a device is not plugged in and a device is registered, warns the user. - * If one type of device is registered but another is plugged in, warns the user. - * - * 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). - * - * \param port - * The V5 port number from 1-21 - * \param expected_t - * The expected type (i.e., the type of function being called. If - * E_DEVICE_NONE, indicates that background processing is calling this, - * and a mismatch will only occur if there is an actual discrepancy - * between what is registered and what is plugged in. - * - * \return 0 if the device registered matches the device plugged and the - * expected device matches both of those or is E_DEVICE_NONE, 1 if the - * registered device is not plugged in, and 2 if there is a mismatch. PROS_ERR - * on exception. - */ -int32_t registry_validate_binding(uint8_t port, v5_device_e_t expected_t); - -#ifdef __cplusplus -} -#undef v5_device_e_t -#endif diff --git a/include/vdml/vdml.h b/include/vdml/vdml.h deleted file mode 100644 index cb7248a7..00000000 --- a/include/vdml/vdml.h +++ /dev/null @@ -1,244 +0,0 @@ -/** - * \file vdml/vdml.h - * - * This file contains all types and functions used throughout multiple VDML - * (Vex Data Management Layer) files. - * - * \copyright Copyright (c) 2017-2024, 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/. - */ - -#pragma once - -#include -#include -#include "vdml/registry.h" - -#ifdef __cplusplus -#define v5_device_e_t pros::c::v5_device_e_t -#endif - -#ifdef __cplusplus -extern "C" { -#endif - -/** - * Macro, returns true if the port is in range of user configurable ports, - * false otherwise. - */ -#define VALIDATE_PORT_NO(PORT) ((PORT) >= 0 && (PORT) < NUM_V5_PORTS) - -#define VALIDATE_PORT_NO_INTERNAL(PORT) ((PORT) >= 0 && (PORT) < V5_MAX_DEVICE_PORTS) - -/** - * Macro that handles error checking, sanity checking, automatic registration, - * and mutex taking for all of the motor wrapper functions. - * If port is out of range, the calling function sets errno and returns. - * If a port isn't yet registered, it registered as a motor automatically. - * If a mutex cannot be taken, errno is set to EACCES (access denied) and - * returns. - * - * \param port - * The V5 port number from 0-20 - * \param device_type - * The v5_device_e_t that the port is configured as - * \param error_code - * The error code that return if error checking failed - */ -#define claim_port(port, device_type, error_code) \ - if (registry_validate_binding(port, device_type) != 0) { \ - return error_code; \ - } \ - v5_smart_device_s_t* device = registry_get_device(port); \ - if (!port_mutex_take(port)) { \ - errno = EACCES; \ - return error_code; \ - } - -/** - * Function like claim_port. This macro should only be used in functions - * that return int32_t or enums as PROS_ERR could be returned. - * - * \param port - * The V5 port number from 0-20 - * \param device_type - * The v5_device_e_t that the port is configured as - */ -#define claim_port_i(port, device_type) claim_port(port, device_type, PROS_ERR) - -/** - * Function like claim_port. This macro should only be used in functions - * that return double or float as PROS_ERR_F could be returned. - * - * \param port - * The V5 port number from 0-20 - * \param device_type - * The v5_device_e_t that the port is configured as - */ -#define claim_port_f(port, device_type) claim_port(port, device_type, PROS_ERR_F) - -/** - * A function that executes claim_port and allows you to execute a block of - * code if an error occurs. - * - * 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). - * EACCES - Another resource is currently trying to access the port. - * - * \param port - * The V5 port number from 0-20 - * \param device_type - * The v5_device_e_t that the port is configured as - * - * \return 1 if the operation was successful or 0 if the operation - * failed, setting errno. - */ -int32_t claim_port_try(uint8_t port, v5_device_e_t type); - -/** - * Macro that release the mutex for the given port and sets errno to 0 if the - * function is an accessor wrapper whos return value is PROS_ERR or PROS_ERR_F. - * - * \param port - * The V5 port number from 0-20 - * \param rtn - * The desired return value - * - * \return The rtn parameter - */ -#define return_port(port, rtn) \ - port_mutex_give(port); \ - return rtn; - -/** - * Bitmap to indicate if a port has had an error printed or not. - */ -extern int32_t port_errors; - -/** - * Sets the port's bit to 1, indicating there has already been an error on this - * port. - * - * \param port - * The V5 port number to set from 0-20 - */ -void vdml_set_port_error(uint8_t port); - -/** - * Sets the port's bit to 0, effectively resetting it. - * - * \param port - * The V5 port number to unset from 0-20 - */ -void vdml_unset_port_error(uint8_t port); - -/** - * Gets the error bit for the port, indicating whether or not there has been an - * error on this port. - * - * \param port - * The V5 port number to check from 0-20 - * - * \return True if the port's bit is set, false otherwise. - */ -bool vdml_get_port_error(uint8_t port); - -/** - * Claims the mutex for the given port. - * - * Reserves the mutex for this port. Any other tasks trying to access this port - * will block until the mutex is returned. If a higher-priortiy task attempts - * to claim this port, the task which has the port claimed will temporarily be - * raised to an equal priority until the mutex is given, reducing the impact of - * the delay. See FreeRTOS documentation for more details. - * - * This MUST be called before any call to the v5 api to maintain thread saftey. - * - * 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 (0-20). - * - * \param port - * The V5 port number to claim from 0-20 - * - * \return 1 if the mutex was successfully taken, 0 if not, -1 if port is - * invalid. - */ -int port_mutex_take(uint8_t port); - -/** - * Returns the mutex for the given port. - * - * Frees the mutex for this port, allowing other tasks to continue. - * - * WARNING: If a mutex was claimed by a task, this MUST be called immediately - * after the port is no longer needed by that task in order to prevent delays in - * other tasks. - * - * 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 (0-20). - * - * \param port - * The V5 port number to free from 0-20 - */ -int port_mutex_give(uint8_t port); - -/** - * Executes port_mutex_take() for all of the V5 Smart Ports. - */ -void port_mutex_take_all(); - -/** - * Executes port_mutex_give() for all of the V5 Smart Ports. - */ -void port_mutex_give_all(); - -/** - * Obtains a port mutex with bounds checking for V5_MAX_PORTS (32) not user - * exposed device ports (20). Intended for internal usage for protecting - * thread-safety on devices such as the controller and battery - * - * 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 (0-32). - * - * \param port - * The V5 port number from 0-32 - * - * \return True if the mutex was successfully taken, false otherwise. If false - * is returned, then errno is set with a hint about why the the mutex - * couldn't be taken. - */ -int internal_port_mutex_take(uint8_t port); - -/** - * Returns a port mutex with bounds checking for V5_MAX_PORTS (32) not user - * exposed device ports (20). Intended for internal usage for protecting - * thread-safety on devices such as the controller and battery - * - * 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 (0-32). - * - * \param port - * The V5 port number from 0-32 - * - * \return True if the mutex was successfully returned, false otherwise. If - * false is returned, then errno is set with a hint about why the mutex - * couldn't be returned. - */ -int internal_port_mutex_give(uint8_t port); - -#define V5_PORT_BATTERY 24 -#define V5_PORT_CONTROLLER_1 25 -#define V5_PORT_CONTROLLER_2 26 - -#ifdef __cplusplus -} -#undef v5_device_e_t -#endif diff --git a/src/devices/vdml.c b/src/devices/vdml.c deleted file mode 100644 index 1dfbf9d3..00000000 --- a/src/devices/vdml.c +++ /dev/null @@ -1,269 +0,0 @@ -/** - * \file devices/vdml.c - * - * VDML - VEX Data Management Layer - * - * VDML ensures thread saftey for operations on smart devices by maintaining - * an array of RTOS Mutexes and implementing functions to take and give them. - * - * \copyright Copyright (c) 2017-2024, 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 "vdml/vdml.h" -#include "kapi.h" -#include "v5_api.h" -#include "vdml/registry.h" - -#include -#include - -/** - * Bitmap to indicate if a port has had an error printed or not. - */ -int32_t port_errors; - -extern void registry_init(); -extern void port_mutex_init(); - -int32_t claim_port_try(uint8_t port, v5_device_e_t type) { - if (!VALIDATE_PORT_NO(port)) { - errno = ENXIO; - return 0; - } - if (registry_validate_binding(port, type) != 0) { - return 0; - } - if (!port_mutex_take(port)) { - errno = EACCES; - return 0; - } - return 1; -} - -/** - * We have V5_MAX_DEVICE_PORTS so that we can do thread safety on things like - * controllers, batteries which are sort of like smart devices internally to the - * V5 - */ -mutex_t port_mutexes[V5_MAX_DEVICE_PORTS]; // Mutexes for each port -static_sem_s_t port_mutex_bufs[V5_MAX_DEVICE_PORTS]; // Stack mem for rtos - -/** - * Shorcut to initialize all of VDML (mutexes and register) - */ -void vdml_initialize() { - port_mutex_init(); - registry_init(); -} - -/** - * Initializes the mutexes for the motor ports. - * - * Initializes a static array of FreeRTOS mutexes to protect against race - * conditions. For example, we don't want the Background processing task to run - * at the same time that we set a motor, because bad information may be - * returned, or worse. - */ -void port_mutex_init() { - for (int i = 0; i < V5_MAX_DEVICE_PORTS; i++) { - port_mutexes[i] = mutex_create_static(&(port_mutex_bufs[i])); - } -} - -int port_mutex_take(uint8_t port) { - if (port >= V5_MAX_DEVICE_PORTS) { - errno = ENXIO; - return PROS_ERR; - } - return xTaskGetSchedulerState() != taskSCHEDULER_RUNNING || mutex_take(port_mutexes[port], TIMEOUT_MAX); -} - -int internal_port_mutex_take(uint8_t port) { - if (port >= V5_MAX_DEVICE_PORTS) { - errno = ENXIO; - return PROS_ERR; - } - return mutex_take(port_mutexes[port], TIMEOUT_MAX); -} - -static inline char* print_num(char* buff, int num) { - *buff++ = (num / 10) + '0'; - *buff++ = (num % 10) + '0'; - return buff; -} - -int port_mutex_give(uint8_t port) { - if (port >= V5_MAX_DEVICE_PORTS) { - errno = ENXIO; - return PROS_ERR; - } - return xTaskGetSchedulerState() != taskSCHEDULER_RUNNING || mutex_give(port_mutexes[port]); -} - -int internal_port_mutex_give(uint8_t port) { - if (port >= V5_MAX_DEVICE_PORTS) { - errno = ENXIO; - return PROS_ERR; - } - return mutex_give(port_mutexes[port]); -} - -void port_mutex_take_all() { - for (int i = 0; i < V5_MAX_DEVICE_PORTS; i++) { - port_mutex_take(i); - } -} - -void port_mutex_give_all() { - for (int i = 0; i < V5_MAX_DEVICE_PORTS; i++) { - port_mutex_give(i); - } -} - -void vdml_set_port_error(uint8_t port) { - if (VALIDATE_PORT_NO(port)) { - port_errors |= (1 << port); - } -} - -void vdml_unset_port_error(uint8_t port) { - if (VALIDATE_PORT_NO(port)) { - port_errors &= ~(1 << port); - } -} - -bool vdml_get_port_error(uint8_t port) { - if (VALIDATE_PORT_NO(port)) { - return (port_errors >> port) & 1; - } else { - return false; - } -} - -#if 0 -void vdml_reset_port_error() { - port_errors = 0; -} -#endif - -/** - * Background processing function for the VDML system. - * - * This function should be called by the system daemon approximately every - * 2 milliseconds. - * - * Updates the registry type array, detecting what devices are actually - * plugged in according to the system, then compares that with the registry - * records. - * - * On warnings, no operation is performed. - */ -void vdml_background_processing() { - -// We're not removing this outright since we want to revisit the idea of logging -// the errors with devices in the future -#if 0 - static int32_t last_port_errors = 0; - static int cycle = 0; - cycle++; - if (cycle % 5000 == 0) { - vdml_reset_port_error(); - last_port_errors = 0; - } -#endif - - // Refresh actual device types. - registry_update_types(); - -#if 0 - // Validate the ports. Warn if mismatch. - uint8_t error_arr[NUM_V5_PORTS]; - int num_errors = 0; - int mismatch_errors = 0; - for (int i = 0; i < NUM_V5_PORTS; i++) { - error_arr[i] = registry_validate_binding(i, E_DEVICE_NONE); - if (error_arr[i] != 0) num_errors++; - if (error_arr[i] == 2) mismatch_errors++; - } - // Every 50 ms - if (cycle % 50 == 0) { - if (last_port_errors == port_errors) { - goto end_render_errors; - } - char line[50]; - char* line_ptr = line; - if (num_errors == 0) - line[0] = (char)0; - else if (num_errors <= 6) { - // If we have 1-6 total errors (unplugged + mismatch), we can - // display a line indicating the ports where these errors occur - strcpy(line_ptr, "PORTS"); - line_ptr += 5; // 5 is length of "PORTS" - if (mismatch_errors != 0) { - strcpy(line_ptr, " MISMATCHED: "); - line_ptr += 13; // 13 is length of previous string - for (int i = 0; i < NUM_V5_PORTS; i++) { - if (error_arr[i] == 2) { - line_ptr = print_num(line_ptr, i + 1); - *line_ptr++ = ','; - } - } - line_ptr--; - } - if (num_errors != mismatch_errors) { - strcpy(line_ptr, " UNPLUGGED: "); - line_ptr += 12; // 12 is length of previous string - for (int i = 0; i < NUM_V5_PORTS; i++) { - if (error_arr[i] == 1) { - line_ptr = print_num(line_ptr, i + 1); - *line_ptr++ = ','; - } - } - line_ptr--; - } - } else { - /* If we have > 6 errors, we display the following: - * PORT ERRORS: 1..... 6..... 11..... 16..... - * where each . represents a port. A '.' indicates - * there is no error on that port, a 'U' indicates - * the registry expected a device there but there isn't - * one, and a 'M' indicates the plugged in devices doesn't - * match what we expect. The numbers are just a visual reference - * to aid in determining what ports have errors. - */ - strcpy(line_ptr, "PORT ERRORS:"); - line_ptr += 12; // 12 is length of previous string - for (int i = 0; i < NUM_V5_PORTS; i++) { - if (i % 5 == 0) { - *line_ptr++ = ' '; - line_ptr = print_num(line_ptr, i + 1); - } - switch (error_arr[i]) { - case 0: - *line_ptr++ = '.'; - break; - case 1: - *line_ptr++ = 'U'; - break; - case 2: - *line_ptr++ = 'M'; - break; - // Should never happen - default: - *line_ptr++ = '?'; - break; - } - } - } - // Null terminate the string - *line_ptr = '\0'; - - end_render_errors: - last_port_errors = port_errors; - } -#endif -} From b609bc61427c9077b62ffc81d5ca04fffd8acda7 Mon Sep 17 00:00:00 2001 From: Exortions <75327059+Exortions@users.noreply.github.com> Date: Wed, 26 Feb 2025 22:01:34 -0800 Subject: [PATCH 02/11] =?UTF-8?q?=E2=9C=A8=20New=20main=20vdml=20header?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- include/vdml/vdml.hpp | 14 ++++++++++++++ 1 file changed, 14 insertions(+) create mode 100644 include/vdml/vdml.hpp diff --git a/include/vdml/vdml.hpp b/include/vdml/vdml.hpp new file mode 100644 index 00000000..03d573bc --- /dev/null +++ b/include/vdml/vdml.hpp @@ -0,0 +1,14 @@ +#ifndef VDML_HPP +#define VDML_HPP + +#include +#include + +namespace vdml { +extern std::array port_mutexes; + +std::mutex& smart_port_mutex(int port); + +} // namespace vdml + +#endif // VDML_HPP \ No newline at end of file From 7af7833e73371dc5af19decf4bd1144ee4959adf Mon Sep 17 00:00:00 2001 From: Exortions <75327059+Exortions@users.noreply.github.com> Date: Wed, 26 Feb 2025 22:03:26 -0800 Subject: [PATCH 03/11] =?UTF-8?q?=E2=99=BB=EF=B8=8F=20Refactor=20vdml=20na?= =?UTF-8?q?mespace=20under=20zest=20namespace?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- include/vdml/vdml.hpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/include/vdml/vdml.hpp b/include/vdml/vdml.hpp index 03d573bc..7735cb65 100644 --- a/include/vdml/vdml.hpp +++ b/include/vdml/vdml.hpp @@ -4,11 +4,13 @@ #include #include +namespace zest { namespace vdml { extern std::array port_mutexes; std::mutex& smart_port_mutex(int port); } // namespace vdml +} // namespace zest #endif // VDML_HPP \ No newline at end of file From e3f688590a6537366803b7153c226a55bbf92e8e Mon Sep 17 00:00:00 2001 From: Exortions <75327059+Exortions@users.noreply.github.com> Date: Wed, 26 Feb 2025 22:30:45 -0800 Subject: [PATCH 04/11] =?UTF-8?q?=E2=9C=A8=20Implement=20registry?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- include/vdml/registry.hpp | 38 ++++++++++++++++++++++++++++++++++++++ include/vdml/vdml.hpp | 2 +- 2 files changed, 39 insertions(+), 1 deletion(-) create mode 100644 include/vdml/registry.hpp diff --git a/include/vdml/registry.hpp b/include/vdml/registry.hpp new file mode 100644 index 00000000..6cf7e395 --- /dev/null +++ b/include/vdml/registry.hpp @@ -0,0 +1,38 @@ +#ifndef VDML_REGISTRY_HPP +#define VDML_REGISTRY_HPP + +#include +#include + +namespace zest { +namespace vdml { +enum class DeviceType { + NONE = 0, + MOTOR = 2, + ROTATION = 4, + IMU = 6, + DISTANCE = 7, + RADIO = 8, + VISION = 11, + ADI = 12, + OPTICAL = 16, + GPS = 20, + AIVISION = 29, + SERIAL = 129, + UNDEFINED = 255 +}; + +struct DeviceInfo { + DeviceType type; + uint32_t data; +}; + +extern std::array, 32> device_registry; + +void update_registry(); +std::optional get_device_info(int port); +bool validate_device(int port, DeviceType expected); +} // namespace vdml +} // namespace zest + +#endif \ No newline at end of file diff --git a/include/vdml/vdml.hpp b/include/vdml/vdml.hpp index 7735cb65..0017becc 100644 --- a/include/vdml/vdml.hpp +++ b/include/vdml/vdml.hpp @@ -6,7 +6,7 @@ namespace zest { namespace vdml { -extern std::array port_mutexes; +extern std::array device_mutexes; std::mutex& smart_port_mutex(int port); From 332aefc4f97196c453850184e5c31ee902125e1c Mon Sep 17 00:00:00 2001 From: Exortions Date: Thu, 27 Feb 2025 12:32:29 -0800 Subject: [PATCH 05/11] =?UTF-8?q?=E2=9C=A8=20Begin=20to=20implement=20regi?= =?UTF-8?q?stry?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- include/vdml/registry.hpp | 15 ++++++-- include/vdml/vdml.hpp | 9 ++++- src/devices/vdml.cpp | 31 ++++++++++++++++ src/devices/vdml_registry.cpp | 69 +++++++++++++++++++++++++++++++++++ 4 files changed, 119 insertions(+), 5 deletions(-) create mode 100644 src/devices/vdml.cpp create mode 100644 src/devices/vdml_registry.cpp diff --git a/include/vdml/registry.hpp b/include/vdml/registry.hpp index 6cf7e395..82a3f5ca 100644 --- a/include/vdml/registry.hpp +++ b/include/vdml/registry.hpp @@ -4,6 +4,9 @@ #include #include +#include "v5_apitypes.h" +#include "vdml/vdml.hpp" + namespace zest { namespace vdml { enum class DeviceType { @@ -17,7 +20,7 @@ enum class DeviceType { ADI = 12, OPTICAL = 16, GPS = 20, - AIVISION = 29, + AI_VISION = 29, SERIAL = 129, UNDEFINED = 255 }; @@ -27,11 +30,15 @@ struct DeviceInfo { uint32_t data; }; -extern std::array, 32> device_registry; +static std::array, MAX_DEVICE_PORTS> device_registry; +static V5_DeviceType registry_types[MAX_DEVICE_PORTS]; + +void initialize_registry(); void update_registry(); -std::optional get_device_info(int port); -bool validate_device(int port, DeviceType expected); +std::optional set_device(uint8_t port, DeviceType type); +std::optional get_device(uint8_t port); +bool validate_device(uint8_t port, DeviceType expected); } // namespace vdml } // namespace zest diff --git a/include/vdml/vdml.hpp b/include/vdml/vdml.hpp index 0017becc..90bad22c 100644 --- a/include/vdml/vdml.hpp +++ b/include/vdml/vdml.hpp @@ -6,9 +6,16 @@ namespace zest { namespace vdml { -extern std::array device_mutexes; +constexpr uint8_t MAX_DEVICE_PORTS = 32; + +extern std::array device_mutexes; std::mutex& smart_port_mutex(int port); +std::mutex create_mutex(); + +void initialize_vdml(); + +bool is_valid_port(uint8_t port); } // namespace vdml } // namespace zest diff --git a/src/devices/vdml.cpp b/src/devices/vdml.cpp new file mode 100644 index 00000000..25fadad3 --- /dev/null +++ b/src/devices/vdml.cpp @@ -0,0 +1,31 @@ +#include "vdml/vdml.hpp" + +#include +#include + +#include "vdml/registry.hpp" + +// initialize the device mutex array +std::array zest::vdml::device_mutexes; + +std::mutex zest::vdml::create_mutex() { + // for now just a stub, as don't know what type of mutex will be implemented yet + return std::mutex(); +} + +void zest::vdml::initialize_vdml() { + // initialize the device (port) mutexes + for (int i = 0; i < zest::vdml::MAX_DEVICE_PORTS; i++) zest::vdml::device_mutexes[i] = create_mutex(); + + // initialize ADI port mutexes +} + +std::mutex& zest::vdml::smart_port_mutex(int8_t port) { + port = abs(port); // prevent negative port + + return zest::vdml::device_mutexes[port]; +} + +bool is_valid_port(uint8_t port) { + return port > 0 && port <= 21; +} \ No newline at end of file diff --git a/src/devices/vdml_registry.cpp b/src/devices/vdml_registry.cpp new file mode 100644 index 00000000..95176750 --- /dev/null +++ b/src/devices/vdml_registry.cpp @@ -0,0 +1,69 @@ +#include + +#include "v5_api.h" +#include "vdml/registry.hpp" +#include "vdml/vdml.hpp" + +using namespace zest::vdml; + +void initialize_registry() { + // update registry types + update_registry(); + for (int i = 0; i < 22; i++) { + device_registry[i]->type = (DeviceType)registry_types[i]; + device_registry[i]->data = (int)vexDeviceGetByIndex(i); + } +} + +void update_registry() { + vexDeviceGetStatus(registry_types); +} + +std::optional set_device(uint8_t port, DeviceType type) { + // TODO: add error messages + // invalid port + if (!is_valid_port(port)) return std::nullopt; + // in use + if (device_registry[port]->type != DeviceType::NONE) return std::nullopt; + // device type mismatch + if ((DeviceType)registry_types[port] != type && (DeviceType)registry_types[port] != DeviceType::NONE) + return std::nullopt; + + DeviceInfo device = {.type = type, .data = (uint32_t)vexDeviceGetByIndex(port)}; + + device_registry[port] = device; + + return device; +} + +std::optional get_device(uint8_t port) { + if (!is_valid_port(port)) return std::nullopt; + + return device_registry[port]; +} + +bool validate_device(uint8_t port, DeviceType expected) { + if (!is_valid_port(port)) return false; + + DeviceType registered = device_registry[port]->type; + DeviceType actual = (DeviceType)registry_types[port]; + + // automatically register the port, if needed + if (registered == DeviceType::NONE && actual != DeviceType::NONE) { + zest::vdml::set_device(port, actual); + registered = device_registry[port]->type; + } + + // TODO: add error / warn messages + + if ((expected == registered || expected == zest::vdml::DeviceType::NONE) && registered == actual) { + // all good + return true; + } else if (actual == zest::vdml::DeviceType::NONE) { + // no device + return false; + } else { + // device type mismatch + return false; + } +} From 9cbd6e9e856b51e563ca1e49ebe2b39119e15055 Mon Sep 17 00:00:00 2001 From: Exortions Date: Fri, 28 Feb 2025 09:51:23 -0800 Subject: [PATCH 06/11] =?UTF-8?q?=F0=9F=8E=A8=20Update=20VDML=20to=20new?= =?UTF-8?q?=20style=20guide?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- include/vdml/registry.hpp | 40 ++++++++-------- include/vdml/vdml.hpp | 7 ++- src/devices/vdml.cpp | 21 +++++---- src/devices/vdml_registry.cpp | 88 +++++++++++++++++++---------------- 4 files changed, 81 insertions(+), 75 deletions(-) diff --git a/include/vdml/registry.hpp b/include/vdml/registry.hpp index 82a3f5ca..9b319d07 100644 --- a/include/vdml/registry.hpp +++ b/include/vdml/registry.hpp @@ -1,33 +1,33 @@ #ifndef VDML_REGISTRY_HPP #define VDML_REGISTRY_HPP -#include -#include - #include "v5_apitypes.h" #include "vdml/vdml.hpp" +#include +#include + namespace zest { namespace vdml { enum class DeviceType { - NONE = 0, - MOTOR = 2, - ROTATION = 4, - IMU = 6, - DISTANCE = 7, - RADIO = 8, - VISION = 11, - ADI = 12, - OPTICAL = 16, - GPS = 20, - AI_VISION = 29, - SERIAL = 129, - UNDEFINED = 255 + NONE = 0, + MOTOR = 2, + ROTATION = 4, + IMU = 6, + DISTANCE = 7, + RADIO = 8, + VISION = 11, + ADI = 12, + OPTICAL = 16, + GPS = 20, + AI_VISION = 29, + SERIAL = 129, + UNDEFINED = 255 }; struct DeviceInfo { - DeviceType type; - uint32_t data; + DeviceType type; + uint32_t data; }; static std::array, MAX_DEVICE_PORTS> device_registry; @@ -39,7 +39,7 @@ void update_registry(); std::optional set_device(uint8_t port, DeviceType type); std::optional get_device(uint8_t port); bool validate_device(uint8_t port, DeviceType expected); -} // namespace vdml -} // namespace zest +} // namespace vdml +} // namespace zest #endif \ No newline at end of file diff --git a/include/vdml/vdml.hpp b/include/vdml/vdml.hpp index 90bad22c..cdf43bb6 100644 --- a/include/vdml/vdml.hpp +++ b/include/vdml/vdml.hpp @@ -16,8 +16,7 @@ std::mutex create_mutex(); void initialize_vdml(); bool is_valid_port(uint8_t port); +} // namespace vdml +} // namespace zest -} // namespace vdml -} // namespace zest - -#endif // VDML_HPP \ No newline at end of file +#endif // VDML_HPP \ No newline at end of file diff --git a/src/devices/vdml.cpp b/src/devices/vdml.cpp index 25fadad3..da2dcd7f 100644 --- a/src/devices/vdml.cpp +++ b/src/devices/vdml.cpp @@ -1,31 +1,32 @@ #include "vdml/vdml.hpp" +#include "vdml/registry.hpp" + #include #include -#include "vdml/registry.hpp" - // initialize the device mutex array std::array zest::vdml::device_mutexes; std::mutex zest::vdml::create_mutex() { - // for now just a stub, as don't know what type of mutex will be implemented yet - return std::mutex(); + // for now just a stub, as don't know what type of mutex will be implemented yet + return std::mutex(); } void zest::vdml::initialize_vdml() { - // initialize the device (port) mutexes - for (int i = 0; i < zest::vdml::MAX_DEVICE_PORTS; i++) zest::vdml::device_mutexes[i] = create_mutex(); + // initialize the device (port) mutexes + for (int i = 0; i < zest::vdml::MAX_DEVICE_PORTS; i++) + zest::vdml::device_mutexes[i] = create_mutex(); - // initialize ADI port mutexes + // initialize ADI port mutexes } std::mutex& zest::vdml::smart_port_mutex(int8_t port) { - port = abs(port); // prevent negative port + port = abs(port); // prevent negative port - return zest::vdml::device_mutexes[port]; + return zest::vdml::device_mutexes[port]; } bool is_valid_port(uint8_t port) { - return port > 0 && port <= 21; + return port > 0 && port <= 21; } \ No newline at end of file diff --git a/src/devices/vdml_registry.cpp b/src/devices/vdml_registry.cpp index 95176750..94593860 100644 --- a/src/devices/vdml_registry.cpp +++ b/src/devices/vdml_registry.cpp @@ -1,69 +1,75 @@ -#include - #include "v5_api.h" #include "vdml/registry.hpp" #include "vdml/vdml.hpp" +#include + using namespace zest::vdml; void initialize_registry() { - // update registry types - update_registry(); - for (int i = 0; i < 22; i++) { - device_registry[i]->type = (DeviceType)registry_types[i]; - device_registry[i]->data = (int)vexDeviceGetByIndex(i); - } + // update registry types + update_registry(); + for (int i = 0; i < 22; i++) { + device_registry[i]->type = (DeviceType)registry_types[i]; + device_registry[i]->data = (int)vexDeviceGetByIndex(i); + } } void update_registry() { - vexDeviceGetStatus(registry_types); + vexDeviceGetStatus(registry_types); } std::optional set_device(uint8_t port, DeviceType type) { - // TODO: add error messages - // invalid port - if (!is_valid_port(port)) return std::nullopt; - // in use - if (device_registry[port]->type != DeviceType::NONE) return std::nullopt; - // device type mismatch - if ((DeviceType)registry_types[port] != type && (DeviceType)registry_types[port] != DeviceType::NONE) - return std::nullopt; + // TODO: add error messages + // invalid port + if (!is_valid_port(port)) + return std::nullopt; + // in use + if (device_registry[port]->type != DeviceType::NONE) + return std::nullopt; + // device type mismatch + if ((DeviceType)registry_types[port] != type + && (DeviceType)registry_types[port] != DeviceType::NONE) + return std::nullopt; - DeviceInfo device = {.type = type, .data = (uint32_t)vexDeviceGetByIndex(port)}; + DeviceInfo device = {.type = type, .data = (uint32_t)vexDeviceGetByIndex(port)}; - device_registry[port] = device; + device_registry[port] = device; - return device; + return device; } std::optional get_device(uint8_t port) { - if (!is_valid_port(port)) return std::nullopt; + if (!is_valid_port(port)) + return std::nullopt; - return device_registry[port]; + return device_registry[port]; } bool validate_device(uint8_t port, DeviceType expected) { - if (!is_valid_port(port)) return false; + if (!is_valid_port(port)) + return false; - DeviceType registered = device_registry[port]->type; - DeviceType actual = (DeviceType)registry_types[port]; + DeviceType registered = device_registry[port]->type; + DeviceType actual = (DeviceType)registry_types[port]; - // automatically register the port, if needed - if (registered == DeviceType::NONE && actual != DeviceType::NONE) { - zest::vdml::set_device(port, actual); - registered = device_registry[port]->type; - } + // automatically register the port, if needed + if (registered == DeviceType::NONE && actual != DeviceType::NONE) { + zest::vdml::set_device(port, actual); + registered = device_registry[port]->type; + } - // TODO: add error / warn messages + // TODO: add error / warn messages - if ((expected == registered || expected == zest::vdml::DeviceType::NONE) && registered == actual) { - // all good - return true; - } else if (actual == zest::vdml::DeviceType::NONE) { - // no device - return false; - } else { - // device type mismatch - return false; - } + if ((expected == registered || expected == zest::vdml::DeviceType::NONE) + && registered == actual) { + // all good + return true; + } else if (actual == zest::vdml::DeviceType::NONE) { + // no device + return false; + } else { + // device type mismatch + return false; + } } From af4a165b09df891c4420923740cdb0847ccfd586 Mon Sep 17 00:00:00 2001 From: Exortions Date: Fri, 28 Feb 2025 09:58:54 -0800 Subject: [PATCH 07/11] =?UTF-8?q?=F0=9F=92=A9=20[temp]=20add=20stubs=20for?= =?UTF-8?q?=20compilation?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- include/vdml/registry.hpp | 13 +++++++++++++ include/vdml/vdml.hpp | 34 ++++++++++++++++++++++++++++++++++ 2 files changed, 47 insertions(+) diff --git a/include/vdml/registry.hpp b/include/vdml/registry.hpp index 9b319d07..ae8357a1 100644 --- a/include/vdml/registry.hpp +++ b/include/vdml/registry.hpp @@ -42,4 +42,17 @@ bool validate_device(uint8_t port, DeviceType expected); } // namespace vdml } // namespace zest +// STUBS SO OLD VDML CODE COMPILES + +typedef struct { + pros::c::v5_device_e_t device_type; + V5_DeviceT device_info; + uint8_t pad[128]; // 16 bytes in adi_data_s_t times 8 ADI Ports = 128 +} v5_smart_device_s_t; + +void registry_update_types(); +v5_smart_device_s_t* registry_get_device(uint8_t port); +v5_smart_device_s_t* registry_get_device_internal(uint8_t port); +int32_t registry_validate_binding(uint8_t port, pros::c::v5_device_e_t expected_t); + #endif \ No newline at end of file diff --git a/include/vdml/vdml.hpp b/include/vdml/vdml.hpp index cdf43bb6..b5d2e511 100644 --- a/include/vdml/vdml.hpp +++ b/include/vdml/vdml.hpp @@ -1,6 +1,8 @@ #ifndef VDML_HPP #define VDML_HPP +#include "pros/device.h" + #include #include @@ -19,4 +21,36 @@ bool is_valid_port(uint8_t port); } // namespace vdml } // namespace zest +// STUBS SO OLD VDML CODE COMPILES +#define VALIDATE_PORT_NO(PORT) ((PORT) >= 0 && (PORT) < NUM_V5_PORTS) +#define VALIDATE_PORT_NO_INTERNAL(PORT) ((PORT) >= 0 && (PORT) < V5_MAX_DEVICE_PORTS) +#define claim_port(port, device_type, error_code) \ + if (registry_validate_binding(port, device_type) != 0) { \ + return error_code; \ + } \ + v5_smart_device_s_t* device = registry_get_device(port); \ + if (!port_mutex_take(port)) { \ + errno = EACCES; \ + return error_code; \ + } +#define claim_port_i(port, device_type) claim_port(port, device_type, PROS_ERR) +#define claim_port_f(port, device_type) claim_port(port, device_type, PROS_ERR_F) +int32_t claim_port_try(uint8_t port, pros::c::v5_device_e_t type); +#define return_port(port, rtn) \ + port_mutex_give(port); \ + return rtn; +extern int32_t port_errors; +void vdml_set_port_error(uint8_t port); +void vdml_unset_port_error(uint8_t port); +bool vdml_get_port_error(uint8_t port); +int port_mutex_take(uint8_t port); +int port_mutex_give(uint8_t port); +void port_mutex_take_all(); +void port_mutex_give_all(); +int internal_port_mutex_take(uint8_t port); +int internal_port_mutex_give(uint8_t port); +#define V5_PORT_BATTERY 24 +#define V5_PORT_CONTROLLER_1 25 +#define V5_PORT_CONTROLLER_2 26 + #endif // VDML_HPP \ No newline at end of file From 8982fc5d0ce04ffeedd2fd506bc4458d8627ae0c Mon Sep 17 00:00:00 2001 From: Exortions Date: Fri, 28 Feb 2025 10:16:39 -0800 Subject: [PATCH 08/11] =?UTF-8?q?=F0=9F=9A=9A=20Rename=20to=20new=20vdml?= =?UTF-8?q?=20headers=20in=20stubs?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/devices/battery.c | 58 +-- src/devices/controller.c | 242 +++++----- src/devices/registry.c | 204 ++++----- src/devices/vdml_adi.c | 93 ++-- src/devices/vdml_ai_vision.c | 222 +++++----- src/devices/vdml_ai_vision.cpp | 69 +-- src/devices/vdml_device.c | 16 +- src/devices/vdml_device.cpp | 60 +-- src/devices/vdml_distance.c | 52 +-- src/devices/vdml_distance.cpp | 46 +- src/devices/vdml_ext_adi.c | 775 ++++++++++++++++++--------------- src/devices/vdml_gps.c | 305 ++++++------- src/devices/vdml_gps.cpp | 111 ++--- src/devices/vdml_imu.c | 616 +++++++++++++------------- src/devices/vdml_imu.cpp | 146 +++---- src/devices/vdml_link.c | 104 +++-- src/devices/vdml_link.cpp | 28 +- src/devices/vdml_motors.c | 368 ++++++++-------- src/devices/vdml_motors.cpp | 681 +++++++++++++++-------------- src/devices/vdml_optical.c | 195 +++++---- src/devices/vdml_optical.cpp | 93 ++-- src/devices/vdml_rotation.c | 89 ++-- src/devices/vdml_rotation.cpp | 71 +-- src/devices/vdml_serial.c | 116 ++--- src/devices/vdml_serial.cpp | 38 +- src/devices/vdml_vision.c | 629 ++++++++++++++------------ src/devices/vdml_vision.cpp | 178 +++++--- src/system/dev/dev_driver.c | 216 ++++----- 28 files changed, 3050 insertions(+), 2771 deletions(-) diff --git a/src/devices/battery.c b/src/devices/battery.c index cabdd484..36c83ef4 100644 --- a/src/devices/battery.c +++ b/src/devices/battery.c @@ -13,44 +13,44 @@ #include "kapi.h" #include "v5_api.h" -#include "vdml/vdml.h" +#include "vdml/vdml.hpp" int32_t battery_get_voltage(void) { - if (!internal_port_mutex_take(V5_PORT_BATTERY)) { - errno = EACCES; - return PROS_ERR; - } - double rtn = vexBatteryVoltageGet(); - internal_port_mutex_give(V5_PORT_BATTERY); - return rtn; + if (!internal_port_mutex_take(V5_PORT_BATTERY)) { + errno = EACCES; + return PROS_ERR; + } + double rtn = vexBatteryVoltageGet(); + internal_port_mutex_give(V5_PORT_BATTERY); + return rtn; } int32_t battery_get_current(void) { - if (!internal_port_mutex_take(V5_PORT_BATTERY)) { - errno = EACCES; - return PROS_ERR; - } - double rtn = vexBatteryCurrentGet(); - internal_port_mutex_give(V5_PORT_BATTERY); - return rtn; + if (!internal_port_mutex_take(V5_PORT_BATTERY)) { + errno = EACCES; + return PROS_ERR; + } + double rtn = vexBatteryCurrentGet(); + internal_port_mutex_give(V5_PORT_BATTERY); + return rtn; } double battery_get_temperature(void) { - if (!internal_port_mutex_take(V5_PORT_BATTERY)) { - errno = EACCES; - return PROS_ERR_F; - } - double rtn = vexBatteryTemperatureGet(); - internal_port_mutex_give(V5_PORT_BATTERY); - return rtn; + if (!internal_port_mutex_take(V5_PORT_BATTERY)) { + errno = EACCES; + return PROS_ERR_F; + } + double rtn = vexBatteryTemperatureGet(); + internal_port_mutex_give(V5_PORT_BATTERY); + return rtn; } double battery_get_capacity(void) { - if (!internal_port_mutex_take(V5_PORT_BATTERY)) { - errno = EACCES; - return PROS_ERR_F; - } - double rtn = vexBatteryCapacityGet(); - internal_port_mutex_give(V5_PORT_BATTERY); - return rtn; + if (!internal_port_mutex_take(V5_PORT_BATTERY)) { + errno = EACCES; + return PROS_ERR_F; + } + double rtn = vexBatteryCapacityGet(); + internal_port_mutex_give(V5_PORT_BATTERY); + return rtn; } diff --git a/src/devices/controller.c b/src/devices/controller.c index 0e90b413..99eeb5d3 100644 --- a/src/devices/controller.c +++ b/src/devices/controller.c @@ -11,196 +11,198 @@ * file, You can obtain one at http://mozilla.org/MPL/2.0/. */ -#include -#include - #include "kapi.h" #include "v5_api.h" -#include "vdml/vdml.h" +#include "vdml/vdml.hpp" + +#include +#include -#define CONTROLLER_MAX_COLS ( 20U ) -#define CONTROLLER_MAX_CHARS ( 31U ) +#define CONTROLLER_MAX_COLS (20U) +#define CONTROLLER_MAX_CHARS (31U) // From enum in misc.h #define NUM_BUTTONS 13 typedef struct controller_data { - bool button_pressed[NUM_BUTTONS]; + bool button_pressed[NUM_BUTTONS]; } controller_data_s_t; static bool get_button_pressed(int port, int button) { - return ((controller_data_s_t*)registry_get_device_internal(port)->pad)->button_pressed[button]; + return ((controller_data_s_t*)registry_get_device_internal(port)->pad)->button_pressed[button]; } static void set_button_pressed(int port, int button, bool state) { - controller_data_s_t* data = (controller_data_s_t*)registry_get_device_internal(port)->pad; - data->button_pressed[button] = state; + controller_data_s_t* data = (controller_data_s_t*)registry_get_device_internal(port)->pad; + data->button_pressed[button] = state; } int32_t controller_is_connected(controller_id_e_t id) { - uint8_t port; - CONTROLLER_PORT_MUTEX_TAKE(id, port) - int32_t rtn = vexControllerConnectionStatusGet(id); - internal_port_mutex_give(port); - return rtn; + uint8_t port; + CONTROLLER_PORT_MUTEX_TAKE(id, port) + int32_t rtn = vexControllerConnectionStatusGet(id); + internal_port_mutex_give(port); + return rtn; } int32_t controller_get_analog(controller_id_e_t id, controller_analog_e_t channel) { - uint8_t port; - CONTROLLER_PORT_MUTEX_TAKE(id, port) - int32_t rtn = vexControllerGet(id, channel); - internal_port_mutex_give(port); - return rtn; + uint8_t port; + CONTROLLER_PORT_MUTEX_TAKE(id, port) + int32_t rtn = vexControllerGet(id, channel); + internal_port_mutex_give(port); + return rtn; } int32_t controller_get_battery_capacity(controller_id_e_t id) { - uint8_t port; - CONTROLLER_PORT_MUTEX_TAKE(id, port) - int32_t rtn = vexControllerGet(id, BatteryCapacity); - internal_port_mutex_give(port); - return rtn; + uint8_t port; + CONTROLLER_PORT_MUTEX_TAKE(id, port) + int32_t rtn = vexControllerGet(id, BatteryCapacity); + internal_port_mutex_give(port); + return rtn; } int32_t controller_get_battery_level(controller_id_e_t id) { - uint8_t port; - CONTROLLER_PORT_MUTEX_TAKE(id, port) - int32_t rtn = vexControllerGet(id, BatteryLevel); - internal_port_mutex_give(port); - return rtn; + uint8_t port; + CONTROLLER_PORT_MUTEX_TAKE(id, port) + int32_t rtn = vexControllerGet(id, BatteryLevel); + internal_port_mutex_give(port); + return rtn; } int32_t controller_get_digital(controller_id_e_t id, controller_digital_e_t button) { - uint8_t port; - CONTROLLER_PORT_MUTEX_TAKE(id, port) - // the buttons enum starts at 6, the correct place for the libv5rts - int32_t rtn = vexControllerGet(id, button); - internal_port_mutex_give(port); - return rtn; + uint8_t port; + CONTROLLER_PORT_MUTEX_TAKE(id, port) + // the buttons enum starts at 6, the correct place for the libv5rts + int32_t rtn = vexControllerGet(id, button); + internal_port_mutex_give(port); + return rtn; } int32_t controller_get_digital_new_press(controller_id_e_t id, controller_digital_e_t button) { - int32_t pressed = controller_get_digital(id, button); - uint8_t port; - CONTROLLER_PORT_MUTEX_TAKE(id, port) - uint8_t button_num = button - E_CONTROLLER_DIGITAL_L1; - - if (!pressed) { - set_button_pressed(port, button_num, false); - } - if (pressed && !get_button_pressed(port, button_num)) { - // button is currently pressed and was not detected as being pressed during - // last check - set_button_pressed(port, button_num, true); - internal_port_mutex_give(port); - return true; - } else { - internal_port_mutex_give(port); - return false; // button is not pressed or was already detected - } + int32_t pressed = controller_get_digital(id, button); + uint8_t port; + CONTROLLER_PORT_MUTEX_TAKE(id, port) + uint8_t button_num = button - E_CONTROLLER_DIGITAL_L1; + + if (!pressed) { + set_button_pressed(port, button_num, false); + } + if (pressed && !get_button_pressed(port, button_num)) { + // button is currently pressed and was not detected as being pressed during + // last check + set_button_pressed(port, button_num, true); + internal_port_mutex_give(port); + return true; + } else { + internal_port_mutex_give(port); + return false; // button is not pressed or was already detected + } } int32_t controller_set_text(controller_id_e_t id, uint8_t line, uint8_t col, const char* str) { - uint8_t port; - CONTROLLER_PORT_MUTEX_TAKE(id, port) - line++; - if (col >= CONTROLLER_MAX_COLS) - col = CONTROLLER_MAX_COLS; - else - col++; + uint8_t port; + CONTROLLER_PORT_MUTEX_TAKE(id, port) + line++; + if (col >= CONTROLLER_MAX_COLS) + col = CONTROLLER_MAX_COLS; + else + col++; - char* buf = strndup(str, CONTROLLER_MAX_CHARS + 1); + char* buf = strndup(str, CONTROLLER_MAX_CHARS + 1); - uint32_t rtn_val = vexControllerTextSet(id, line, col, buf); - free(buf); - internal_port_mutex_give(port); + uint32_t rtn_val = vexControllerTextSet(id, line, col, buf); + free(buf); + internal_port_mutex_give(port); - if (!rtn_val) { - errno = EAGAIN; - return PROS_ERR; - } - return 1; + if (!rtn_val) { + errno = EAGAIN; + return PROS_ERR; + } + return 1; } int32_t controller_print(controller_id_e_t id, uint8_t line, uint8_t col, const char* fmt, ...) { - uint8_t port; - CONTROLLER_PORT_MUTEX_TAKE(id, port) - line++; - if (col >= CONTROLLER_MAX_COLS) - col = CONTROLLER_MAX_COLS; - else - col++; + uint8_t port; + CONTROLLER_PORT_MUTEX_TAKE(id, port) + line++; + if (col >= CONTROLLER_MAX_COLS) + col = CONTROLLER_MAX_COLS; + else + col++; - va_list args; - va_start(args, fmt); - char* buf = (char*)malloc(CONTROLLER_MAX_CHARS + 1); - vsnprintf(buf, CONTROLLER_MAX_CHARS + 1, fmt, args); + va_list args; + va_start(args, fmt); + char* buf = (char*)malloc(CONTROLLER_MAX_CHARS + 1); + vsnprintf(buf, CONTROLLER_MAX_CHARS + 1, fmt, args); - uint32_t rtn_val = vexControllerTextSet(id, line, col, buf); - free(buf); - va_end(args); + uint32_t rtn_val = vexControllerTextSet(id, line, col, buf); + free(buf); + va_end(args); - internal_port_mutex_give(port); + internal_port_mutex_give(port); - if (!rtn_val) { - errno = EAGAIN; - return PROS_ERR; - } - return 1; + if (!rtn_val) { + errno = EAGAIN; + return PROS_ERR; + } + return 1; } int32_t controller_clear_line(controller_id_e_t id, uint8_t line) { - uint8_t port; - CONTROLLER_PORT_MUTEX_TAKE(id, port); - int32_t rtn; - if (vexSystemVersion() >= 0x1000C38) { - static char* clear = ""; - rtn = vexControllerTextSet(id, ++line, 1, clear); - } else { - static char* clear = " "; - kassert(strlen(clear) == CONTROLLER_MAX_COLS); - rtn = vexControllerTextSet(id, ++line, 1, clear); - } - internal_port_mutex_give(port); - return rtn; + uint8_t port; + CONTROLLER_PORT_MUTEX_TAKE(id, port); + int32_t rtn; + if (vexSystemVersion() >= 0x1000C38) { + static char* clear = ""; + rtn = vexControllerTextSet(id, ++line, 1, clear); + } else { + static char* clear = " "; + kassert(strlen(clear) == CONTROLLER_MAX_COLS); + rtn = vexControllerTextSet(id, ++line, 1, clear); + } + internal_port_mutex_give(port); + return rtn; } int32_t controller_clear(controller_id_e_t id) { - if (vexSystemVersion() > 0x01000000) { - return controller_print(id, -1, 0, ""); - } else { - for (int i = 0; i < 3; i++) { - int32_t rtn = controller_clear_line(id, i); - if (rtn == PROS_ERR) return PROS_ERR; - if (i != 2) delay(55); - } - return 1; - } + if (vexSystemVersion() > 0x01000000) { + return controller_print(id, -1, 0, ""); + } else { + for (int i = 0; i < 3; i++) { + int32_t rtn = controller_clear_line(id, i); + if (rtn == PROS_ERR) + return PROS_ERR; + if (i != 2) + delay(55); + } + return 1; + } } int32_t controller_rumble(controller_id_e_t id, const char* rumble_pattern) { - return controller_set_text(id, 3, 0, rumble_pattern); + return controller_set_text(id, 3, 0, rumble_pattern); } uint8_t competition_get_status(void) { - return vexCompetitionStatus(); + return vexCompetitionStatus(); } uint8_t competition_is_disabled(void) { - return (competition_get_status() & COMPETITION_DISABLED) != 0; + return (competition_get_status() & COMPETITION_DISABLED) != 0; } uint8_t competition_is_connected(void) { - return (competition_get_status() & COMPETITION_CONNECTED) != 0; + return (competition_get_status() & COMPETITION_CONNECTED) != 0; } uint8_t competition_is_autonomous(void) { - return (competition_get_status() & COMPETITION_AUTONOMOUS) != 0; + return (competition_get_status() & COMPETITION_AUTONOMOUS) != 0; } uint8_t competition_is_field(void) { - return ((competition_get_status() & COMPETITION_SYSTEM) != 0) && competition_is_connected(); + 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(); + return ((competition_get_status() & COMPETITION_SYSTEM) == 0) && competition_is_connected(); } diff --git a/src/devices/registry.c b/src/devices/registry.c index 12157abd..e6bc33b1 100644 --- a/src/devices/registry.c +++ b/src/devices/registry.c @@ -12,139 +12,141 @@ * file, You can obtain one at http://mozilla.org/MPL/2.0/. */ -#include -#include +#include "vdml/registry.hpp" #include "api.h" #include "kapi.h" #include "pros/misc.h" #include "v5_api.h" -#include "vdml/registry.h" -#include "vdml/vdml.h" +#include "vdml/vdml.hpp" + +#include +#include static v5_smart_device_s_t registry[V5_MAX_DEVICE_PORTS]; static V5_DeviceType registry_types[V5_MAX_DEVICE_PORTS]; void registry_init() { - int i; - kprint("[VDML][INFO]Initializing registry\n"); - registry_update_types(); - for (i = 0; i < NUM_V5_PORTS; i++) { - registry[i].device_type = (v5_device_e_t)registry_types[i]; - registry[i].device_info = vexDeviceGetByIndex(i); - if (registry[i].device_type != E_DEVICE_NONE) { - kprintf("[VDML][INFO]Register device in port %d", i + 1); - } - } - kprint("[VDML][INFO]Done initializing registry\n"); + int i; + kprint("[VDML][INFO]Initializing registry\n"); + registry_update_types(); + for (i = 0; i < NUM_V5_PORTS; i++) { + registry[i].device_type = (v5_device_e_t)registry_types[i]; + registry[i].device_info = vexDeviceGetByIndex(i); + if (registry[i].device_type != E_DEVICE_NONE) { + kprintf("[VDML][INFO]Register device in port %d", i + 1); + } + } + kprint("[VDML][INFO]Done initializing registry\n"); } void registry_update_types() { - vexDeviceGetStatus(registry_types); + vexDeviceGetStatus(registry_types); } int registry_bind_port(uint8_t port, v5_device_e_t device_type) { - if (!VALIDATE_PORT_NO(port)) { - kprintf("[VDML][ERROR]Registration: Invalid port number %d\n", port + 1); - errno = ENXIO; - return PROS_ERR; - } - if (registry[port].device_type != E_DEVICE_NONE) { - kprintf("[VDML][ERROR]Registration: Port already in use %d\n", port + 1); - errno = EADDRINUSE; - return PROS_ERR; - } - if ((v5_device_e_t)registry_types[port] != device_type && (v5_device_e_t)registry_types[port] != E_DEVICE_NONE) { - kprintf("[VDML][ERROR]Registration: Device mismatch in port %d\n", port + 1); - errno = EADDRINUSE; - return PROS_ERR; - } - kprintf("[VDML][INFO]Registering device in port %d\n", port + 1); - v5_smart_device_s_t device; - device.device_type = device_type; - device.device_info = vexDeviceGetByIndex(port); - registry[port] = device; - return 1; + if (!VALIDATE_PORT_NO(port)) { + kprintf("[VDML][ERROR]Registration: Invalid port number %d\n", port + 1); + errno = ENXIO; + return PROS_ERR; + } + if (registry[port].device_type != E_DEVICE_NONE) { + kprintf("[VDML][ERROR]Registration: Port already in use %d\n", port + 1); + errno = EADDRINUSE; + return PROS_ERR; + } + if ((v5_device_e_t)registry_types[port] != device_type + && (v5_device_e_t)registry_types[port] != E_DEVICE_NONE) { + kprintf("[VDML][ERROR]Registration: Device mismatch in port %d\n", port + 1); + errno = EADDRINUSE; + return PROS_ERR; + } + kprintf("[VDML][INFO]Registering device in port %d\n", port + 1); + v5_smart_device_s_t device; + device.device_type = device_type; + device.device_info = vexDeviceGetByIndex(port); + registry[port] = device; + return 1; } int registry_unbind_port(uint8_t port) { - if (!VALIDATE_PORT_NO(port)) { - errno = ENXIO; - return PROS_ERR; - } - registry[port].device_type = E_DEVICE_NONE; - registry[port].device_info = NULL; - return 1; + if (!VALIDATE_PORT_NO(port)) { + errno = ENXIO; + return PROS_ERR; + } + registry[port].device_type = E_DEVICE_NONE; + registry[port].device_info = NULL; + return 1; } v5_smart_device_s_t* registry_get_device(uint8_t port) { - if (!VALIDATE_PORT_NO(port)) { - errno = ENXIO; - return NULL; - } - return ®istry[port]; + if (!VALIDATE_PORT_NO(port)) { + errno = ENXIO; + return NULL; + } + return ®istry[port]; } v5_smart_device_s_t* registry_get_device_internal(uint8_t port) { - if (!VALIDATE_PORT_NO_INTERNAL(port)) { - errno = ENXIO; - return NULL; - } - return ®istry[port]; + if (!VALIDATE_PORT_NO_INTERNAL(port)) { + errno = ENXIO; + return NULL; + } + return ®istry[port]; } v5_device_e_t registry_get_bound_type(uint8_t port) { - if (!VALIDATE_PORT_NO(port)) { - errno = ENXIO; - return E_DEVICE_UNDEFINED; - } - return registry[port].device_type; + if (!VALIDATE_PORT_NO(port)) { + errno = ENXIO; + return E_DEVICE_UNDEFINED; + } + return registry[port].device_type; } v5_device_e_t registry_get_plugged_type(uint8_t port) { - if (!VALIDATE_PORT_NO(port)) { - errno = ENXIO; - return -1; - } - return registry_types[port]; + if (!VALIDATE_PORT_NO(port)) { + errno = ENXIO; + return -1; + } + return registry_types[port]; } int32_t registry_validate_binding(uint8_t port, v5_device_e_t expected_t) { - if (!VALIDATE_PORT_NO(port)) { - errno = ENXIO; - return PROS_ERR; - } - - // Get the registered and plugged types - v5_device_e_t registered_t = registry_get_bound_type(port); - v5_device_e_t actual_t = registry_get_plugged_type(port); - - // Auto register the port if needed - if (registered_t == E_DEVICE_NONE && actual_t != E_DEVICE_NONE) { - registry_bind_port(port, actual_t); - registered_t = registry_get_bound_type(port); - } - - if ((expected_t == registered_t || expected_t == E_DEVICE_NONE) && registered_t == actual_t) { - // All are same OR expected is none (bgp) AND reg = act. - // All good - vdml_unset_port_error(port); - return 0; - } else if (actual_t == E_DEVICE_NONE) { - // Warn about nothing plugged - if (!vdml_get_port_error(port)) { - kprintf("[VDML][WARNING] No device in port %d. Is it plugged in?\n", port + 1); - vdml_set_port_error(port); - } - errno = ENODEV; - return 1; - } else { - // Warn about a mismatch - if (!vdml_get_port_error(port)) { - kprintf("[VDML][WARNING] Device mismatch in port %d.\n", port + 1); - vdml_set_port_error(port); - } - errno = EADDRINUSE; - return 2; - } + if (!VALIDATE_PORT_NO(port)) { + errno = ENXIO; + return PROS_ERR; + } + + // Get the registered and plugged types + v5_device_e_t registered_t = registry_get_bound_type(port); + v5_device_e_t actual_t = registry_get_plugged_type(port); + + // Auto register the port if needed + if (registered_t == E_DEVICE_NONE && actual_t != E_DEVICE_NONE) { + registry_bind_port(port, actual_t); + registered_t = registry_get_bound_type(port); + } + + if ((expected_t == registered_t || expected_t == E_DEVICE_NONE) && registered_t == actual_t) { + // All are same OR expected is none (bgp) AND reg = act. + // All good + vdml_unset_port_error(port); + return 0; + } else if (actual_t == E_DEVICE_NONE) { + // Warn about nothing plugged + if (!vdml_get_port_error(port)) { + kprintf("[VDML][WARNING] No device in port %d. Is it plugged in?\n", port + 1); + vdml_set_port_error(port); + } + errno = ENODEV; + return 1; + } else { + // Warn about a mismatch + if (!vdml_get_port_error(port)) { + kprintf("[VDML][WARNING] Device mismatch in port %d.\n", port + 1); + vdml_set_port_error(port); + } + errno = EADDRINUSE; + return 2; + } } diff --git a/src/devices/vdml_adi.c b/src/devices/vdml_adi.c index 6c9a898e..f0aa3019 100644 --- a/src/devices/vdml_adi.c +++ b/src/devices/vdml_adi.c @@ -10,145 +10,158 @@ * file, You can obtain one at http://mozilla.org/MPL/2.0/. */ -#include "vdml/registry.h" -#include "vdml/vdml.h" +#include "vdml/registry.hpp" +#include "vdml/vdml.hpp" adi_port_config_e_t adi_port_get_config(uint8_t port) { - return ext_adi_port_get_config(INTERNAL_ADI_PORT, port); + return ext_adi_port_get_config(INTERNAL_ADI_PORT, port); } int32_t adi_port_get_value(uint8_t port) { - return ext_adi_port_get_value(INTERNAL_ADI_PORT, port); + return ext_adi_port_get_value(INTERNAL_ADI_PORT, port); } int32_t adi_port_set_config(uint8_t port, adi_port_config_e_t type) { - return ext_adi_port_set_config(INTERNAL_ADI_PORT, port, type); + return ext_adi_port_set_config(INTERNAL_ADI_PORT, port, type); } int32_t adi_port_set_value(uint8_t port, int32_t value) { - return ext_adi_port_set_value(INTERNAL_ADI_PORT, port, value); + return ext_adi_port_set_value(INTERNAL_ADI_PORT, port, value); } int32_t adi_analog_calibrate(uint8_t port) { - return ext_adi_analog_calibrate(INTERNAL_ADI_PORT, port); + return ext_adi_analog_calibrate(INTERNAL_ADI_PORT, port); } int32_t adi_analog_read(uint8_t port) { - return ext_adi_analog_read(INTERNAL_ADI_PORT, port); + return ext_adi_analog_read(INTERNAL_ADI_PORT, port); } int32_t adi_analog_read_calibrated(uint8_t port) { - return ext_adi_analog_read_calibrated(INTERNAL_ADI_PORT, port); + return ext_adi_analog_read_calibrated(INTERNAL_ADI_PORT, port); } int32_t adi_analog_read_calibrated_HR(uint8_t port) { - return ext_adi_analog_read_calibrated_HR(INTERNAL_ADI_PORT, port); + return ext_adi_analog_read_calibrated_HR(INTERNAL_ADI_PORT, port); } int32_t adi_digital_read(uint8_t port) { - return ext_adi_digital_read(INTERNAL_ADI_PORT, port); + return ext_adi_digital_read(INTERNAL_ADI_PORT, port); } int32_t adi_digital_get_new_press(uint8_t port) { - return ext_adi_digital_get_new_press(INTERNAL_ADI_PORT, port); + return ext_adi_digital_get_new_press(INTERNAL_ADI_PORT, port); } int32_t adi_digital_write(uint8_t port, bool value) { - return ext_adi_digital_write(INTERNAL_ADI_PORT, port, value); + return ext_adi_digital_write(INTERNAL_ADI_PORT, port, value); } int32_t adi_pin_mode(uint8_t port, uint8_t mode) { - return ext_adi_pin_mode(INTERNAL_ADI_PORT, port, mode); + return ext_adi_pin_mode(INTERNAL_ADI_PORT, port, mode); } int32_t adi_motor_set(uint8_t port, int8_t speed) { - return ext_adi_motor_set(INTERNAL_ADI_PORT, port, speed); + return ext_adi_motor_set(INTERNAL_ADI_PORT, port, speed); } int32_t adi_motor_get(uint8_t port) { - return ext_adi_motor_get(INTERNAL_ADI_PORT, port); + return ext_adi_motor_get(INTERNAL_ADI_PORT, port); } int32_t adi_motor_stop(uint8_t port) { - return ext_adi_motor_stop(INTERNAL_ADI_PORT, port); + return ext_adi_motor_stop(INTERNAL_ADI_PORT, port); } adi_encoder_t adi_encoder_init(uint8_t port_top, uint8_t port_bottom, bool reverse) { - return (adi_encoder_t)ext_adi_encoder_init(INTERNAL_ADI_PORT, port_top, port_bottom, reverse); + return (adi_encoder_t)ext_adi_encoder_init(INTERNAL_ADI_PORT, port_top, port_bottom, reverse); } int32_t adi_encoder_get(adi_encoder_t enc) { - return ext_adi_encoder_get((ext_adi_encoder_t)enc); + return ext_adi_encoder_get((ext_adi_encoder_t)enc); } int32_t adi_encoder_reset(adi_encoder_t enc) { - return ext_adi_encoder_reset((ext_adi_encoder_t)enc); + return ext_adi_encoder_reset((ext_adi_encoder_t)enc); } int32_t adi_encoder_shutdown(adi_encoder_t enc) { - return ext_adi_encoder_shutdown((ext_adi_encoder_t)enc); + return ext_adi_encoder_shutdown((ext_adi_encoder_t)enc); } adi_ultrasonic_t adi_ultrasonic_init(uint8_t port_ping, uint8_t port_echo) { - return (adi_ultrasonic_t)ext_adi_ultrasonic_init(INTERNAL_ADI_PORT, port_ping, port_echo); + return (adi_ultrasonic_t)ext_adi_ultrasonic_init(INTERNAL_ADI_PORT, port_ping, port_echo); } int32_t adi_ultrasonic_get(adi_ultrasonic_t ult) { - return ext_adi_ultrasonic_get((ext_adi_ultrasonic_t)ult); + return ext_adi_ultrasonic_get((ext_adi_ultrasonic_t)ult); } int32_t adi_ultrasonic_shutdown(adi_ultrasonic_t ult) { - return ext_adi_ultrasonic_shutdown((ext_adi_ultrasonic_t)ult); + return ext_adi_ultrasonic_shutdown((ext_adi_ultrasonic_t)ult); } adi_gyro_t adi_gyro_init(uint8_t adi_port, double multiplier) { - return (adi_gyro_t)ext_adi_gyro_init(INTERNAL_ADI_PORT, adi_port, multiplier); + return (adi_gyro_t)ext_adi_gyro_init(INTERNAL_ADI_PORT, adi_port, multiplier); } double adi_gyro_get(adi_gyro_t gyro) { - return ext_adi_gyro_get((ext_adi_gyro_t)gyro); + return ext_adi_gyro_get((ext_adi_gyro_t)gyro); } int32_t adi_gyro_reset(adi_gyro_t gyro) { - return ext_adi_gyro_reset((ext_adi_gyro_t)gyro); + return ext_adi_gyro_reset((ext_adi_gyro_t)gyro); } int32_t adi_gyro_shutdown(adi_gyro_t gyro) { - return ext_adi_gyro_shutdown((ext_adi_gyro_t)gyro); + return ext_adi_gyro_shutdown((ext_adi_gyro_t)gyro); } adi_potentiometer_t adi_potentiometer_init(uint8_t port) { - return (adi_potentiometer_t)ext_adi_potentiometer_init(INTERNAL_ADI_PORT, port, E_ADI_POT_EDR); + return (adi_potentiometer_t)ext_adi_potentiometer_init(INTERNAL_ADI_PORT, port, E_ADI_POT_EDR); } -adi_potentiometer_t adi_potentiometer_type_init(uint8_t port, adi_potentiometer_type_e_t potentiometer_type) { - return (adi_potentiometer_t)ext_adi_potentiometer_init(INTERNAL_ADI_PORT, port, potentiometer_type); +adi_potentiometer_t +adi_potentiometer_type_init(uint8_t port, adi_potentiometer_type_e_t potentiometer_type) { + return (adi_potentiometer_t + )ext_adi_potentiometer_init(INTERNAL_ADI_PORT, port, potentiometer_type); } double adi_potentiometer_get_angle(adi_potentiometer_t potentiometer) { - return ext_adi_potentiometer_get_angle((ext_adi_potentiometer_t)potentiometer); + return ext_adi_potentiometer_get_angle((ext_adi_potentiometer_t)potentiometer); } adi_led_t adi_led_init(uint8_t port) { - return (adi_led_t)ext_adi_led_init(INTERNAL_ADI_PORT, port); + return (adi_led_t)ext_adi_led_init(INTERNAL_ADI_PORT, port); } int32_t adi_led_set(adi_led_t led, uint32_t* buffer, uint32_t buffer_length) { - return ext_adi_led_set(led, buffer, buffer_length); + return ext_adi_led_set(led, buffer, buffer_length); } -int32_t adi_led_set_pixel(adi_led_t led, uint32_t* buffer, uint32_t buffer_length, uint32_t color, uint32_t pixel_position) { - return ext_adi_led_set_pixel((ext_adi_led_t)led, buffer, buffer_length, color, pixel_position); +int32_t adi_led_set_pixel( + adi_led_t led, + uint32_t* buffer, + uint32_t buffer_length, + uint32_t color, + uint32_t pixel_position +) { + return ext_adi_led_set_pixel((ext_adi_led_t)led, buffer, buffer_length, color, pixel_position); } int32_t adi_led_set_all(adi_led_t led, uint32_t* buffer, uint32_t buffer_length, uint32_t color) { - return ext_adi_led_set_all((ext_adi_led_t)led, buffer, buffer_length, color); + return ext_adi_led_set_all((ext_adi_led_t)led, buffer, buffer_length, color); } int32_t adi_led_clear_all(adi_led_t led, uint32_t* buffer, uint32_t buffer_length) { - return ext_adi_led_set_all((ext_adi_led_t)led, buffer, buffer_length, 0); + return ext_adi_led_set_all((ext_adi_led_t)led, buffer, buffer_length, 0); } -int32_t adi_led_clear_pixel(adi_led_t led, uint32_t* buffer, uint32_t buffer_length, uint32_t pixel_position) { - return ext_adi_led_set_pixel((ext_adi_led_t)led, buffer, buffer_length, 0, pixel_position); +int32_t adi_led_clear_pixel( + adi_led_t led, + uint32_t* buffer, + uint32_t buffer_length, + uint32_t pixel_position +) { + return ext_adi_led_set_pixel((ext_adi_led_t)led, buffer, buffer_length, 0, pixel_position); } diff --git a/src/devices/vdml_ai_vision.c b/src/devices/vdml_ai_vision.c index 4eed1242..f7bce501 100644 --- a/src/devices/vdml_ai_vision.c +++ b/src/devices/vdml_ai_vision.c @@ -11,160 +11,170 @@ */ #include "pros/ai_vision.h" #include "v5_api.h" -#include "vdml/registry.h" -#include "vdml/vdml.h" - -#define AIVISION_COLOR_ERR_INIT \ - { .id = PROS_ERR_BYTE, .red = PROS_ERR_BYTE, .green = PROS_ERR_BYTE, .blue = PROS_ERR_BYTE, \ - .hue_range = PROS_ERR_F, .saturation_range = PROS_ERR_F \ - } - -#define AIVISION_CODE_ERR_INIT \ - { .id = PROS_ERR_BYTE, .length = PROS_ERR_BYTE, .c1 = PROS_ERR_2_BYTE, .c2 = PROS_ERR_2_BYTE, \ - .c3 = PROS_ERR_2_BYTE, .c4 = PROS_ERR_2_BYTE, .c5 = PROS_ERR_2_BYTE \ - } - -#define AIVISION_OBJECT_ERR_INIT \ - { .id = PROS_ERR_BYTE, .type = PROS_ERR_BYTE } +#include "vdml/registry.hpp" +#include "vdml/vdml.hpp" + +#define AIVISION_COLOR_ERR_INIT \ + {.id = PROS_ERR_BYTE, \ + .red = PROS_ERR_BYTE, \ + .green = PROS_ERR_BYTE, \ + .blue = PROS_ERR_BYTE, \ + .hue_range = PROS_ERR_F, \ + .saturation_range = PROS_ERR_F} + +#define AIVISION_CODE_ERR_INIT \ + {.id = PROS_ERR_BYTE, \ + .length = PROS_ERR_BYTE, \ + .c1 = PROS_ERR_2_BYTE, \ + .c2 = PROS_ERR_2_BYTE, \ + .c3 = PROS_ERR_2_BYTE, \ + .c4 = PROS_ERR_2_BYTE, \ + .c5 = PROS_ERR_2_BYTE} + +#define AIVISION_OBJECT_ERR_INIT {.id = PROS_ERR_BYTE, .type = PROS_ERR_BYTE} int32_t aivision_reset(uint8_t port) { - claim_port_i(port - 1, E_DEVICE_AIVISION); - vexDeviceAiVisionReset(device->device_info); - return_port(port - 1, PROS_SUCCESS); + claim_port_i(port - 1, E_DEVICE_AIVISION); + vexDeviceAiVisionReset(device->device_info); + return_port(port - 1, PROS_SUCCESS); } int32_t aivision_set_enabled_detection_types(uint8_t port, uint8_t bits, uint8_t bitmask) { - claim_port_i(port - 1, E_DEVICE_AIVISION); - vexDeviceAiVisionEnableSet(device->device_info, bits, bitmask); - return_port(port - 1, PROS_SUCCESS); + claim_port_i(port - 1, E_DEVICE_AIVISION); + vexDeviceAiVisionEnableSet(device->device_info, bits, bitmask); + return_port(port - 1, PROS_SUCCESS); } int32_t aivision_get_enabled_detection_types(uint8_t port) { - claim_port_i(port - 1, E_DEVICE_AIVISION); - uint8_t enabled_detection_types = vexDeviceAiVisionEnableGet(device->device_info); - return_port(port - 1, enabled_detection_types); + claim_port_i(port - 1, E_DEVICE_AIVISION); + uint8_t enabled_detection_types = vexDeviceAiVisionEnableGet(device->device_info); + return_port(port - 1, enabled_detection_types); } int32_t aivision_enable_detection_types(uint8_t port, uint8_t types_mask) { - claim_port_i(port - 1, E_DEVICE_AIVISION); - vexDeviceAiVisionEnableSet(device->device_info, types_mask, types_mask); - return_port(port - 1, PROS_SUCCESS); + claim_port_i(port - 1, E_DEVICE_AIVISION); + vexDeviceAiVisionEnableSet(device->device_info, types_mask, types_mask); + return_port(port - 1, PROS_SUCCESS); } int32_t aivision_disable_detection_types(uint8_t port, uint8_t types_mask) { - claim_port_i(port - 1, E_DEVICE_AIVISION); - vexDeviceAiVisionEnableSet(device->device_info, 0, types_mask); - return_port(port - 1, PROS_SUCCESS); + claim_port_i(port - 1, E_DEVICE_AIVISION); + vexDeviceAiVisionEnableSet(device->device_info, 0, types_mask); + return_port(port - 1, PROS_SUCCESS); } int32_t aivision_get_class_name(uint8_t port, int32_t id, uint8_t* class_name) { - claim_port_i(port - 1, E_DEVICE_AIVISION); - vexDeviceAiVisionClassNameGet(device->device_info, id, class_name); - return_port(port - 1, PROS_SUCCESS); + claim_port_i(port - 1, E_DEVICE_AIVISION); + vexDeviceAiVisionClassNameGet(device->device_info, id, class_name); + return_port(port - 1, PROS_SUCCESS); } int32_t aivision_set_color(uint8_t port, aivision_color_s_t* color) { - claim_port_i(port - 1, E_DEVICE_AIVISION); - V5_DeviceAiVisionColor _color; - _color.id = color->id; - _color.red = color->red; - _color.grn = color->green; - _color.blu = color->blue; - _color.hangle = color->hue_range; - _color.hdsat = color->saturation_range; - vexDeviceAiVisionColorSet(device->device_info, &_color); - return_port(port - 1, PROS_SUCCESS); + claim_port_i(port - 1, E_DEVICE_AIVISION); + V5_DeviceAiVisionColor _color; + _color.id = color->id; + _color.red = color->red; + _color.grn = color->green; + _color.blu = color->blue; + _color.hangle = color->hue_range; + _color.hdsat = color->saturation_range; + vexDeviceAiVisionColorSet(device->device_info, &_color); + return_port(port - 1, PROS_SUCCESS); } aivision_color_s_t aivision_get_color(uint8_t port, uint32_t id) { - aivision_color_s_t color = AIVISION_COLOR_ERR_INIT; - if (!claim_port_try(port - 1, E_DEVICE_AIVISION)) { - return color; - } - V5_DeviceAiVisionColor _color; - v5_smart_device_s_t* device = registry_get_device(port - 1); - vexDeviceAiVisionColorGet(device->device_info, id, &_color); - color.id = _color.id; - color.red = _color.red; - color.green = _color.grn; - color.blue = _color.blu; - color.hue_range = _color.hangle; - color.saturation_range = _color.hdsat; - return_port(port - 1, color); + aivision_color_s_t color = AIVISION_COLOR_ERR_INIT; + if (!claim_port_try(port - 1, E_DEVICE_AIVISION)) { + return color; + } + V5_DeviceAiVisionColor _color; + v5_smart_device_s_t* device = registry_get_device(port - 1); + vexDeviceAiVisionColorGet(device->device_info, id, &_color); + color.id = _color.id; + color.red = _color.red; + color.green = _color.grn; + color.blue = _color.blu; + color.hue_range = _color.hangle; + color.saturation_range = _color.hdsat; + return_port(port - 1, color); } int32_t aivision_get_object_count(uint8_t port) { - claim_port_i(port - 1, E_DEVICE_AIVISION); - int32_t result = vexDeviceAiVisionObjectCountGet(device->device_info); - return_port(port - 1, result); + claim_port_i(port - 1, E_DEVICE_AIVISION); + int32_t result = vexDeviceAiVisionObjectCountGet(device->device_info); + return_port(port - 1, result); } double aivision_get_temperature(uint8_t port) { - claim_port_f(port - 1, E_DEVICE_AIVISION); - double result = vexDeviceAiVisionTemperatureGet(device->device_info); - return_port(port - 1, result); + claim_port_f(port - 1, E_DEVICE_AIVISION); + double result = vexDeviceAiVisionTemperatureGet(device->device_info); + return_port(port - 1, result); } int32_t aivision_set_tag_family(uint8_t port, aivision_tag_family_e_t family) { - claim_port_i(port - 1, E_DEVICE_AIVISION); - uint32_t tag_family_flag = (uint32_t)family << 16; - vexDeviceAiVisionModeSet(device->device_info, tag_family_flag | AIVISION_MODE_TAG_SET_BIT); - return_port(port - 1, PROS_SUCCESS); + claim_port_i(port - 1, E_DEVICE_AIVISION); + uint32_t tag_family_flag = (uint32_t)family << 16; + vexDeviceAiVisionModeSet(device->device_info, tag_family_flag | AIVISION_MODE_TAG_SET_BIT); + return_port(port - 1, PROS_SUCCESS); } int32_t aivision_set_usb_bounding_box_overlay(uint8_t port, bool enabled) { - claim_port_i(port - 1, E_DEVICE_AIVISION); - uint32_t mode = vexDeviceAiVisionStatusGet(device->device_info); - if (enabled) { - mode &= 0x7F; - } else { - mode = (mode != 0) | 0x80; - } - mode = (mode << 8) | (1 << 25); - - vexDeviceAiVisionModeSet(device->device_info, mode); - return_port(port - 1, PROS_SUCCESS); + claim_port_i(port - 1, E_DEVICE_AIVISION); + uint32_t mode = vexDeviceAiVisionStatusGet(device->device_info); + if (enabled) { + mode &= 0x7F; + } else { + mode = (mode != 0) | 0x80; + } + mode = (mode << 8) | (1 << 25); + + vexDeviceAiVisionModeSet(device->device_info, mode); + return_port(port - 1, PROS_SUCCESS); } aivision_object_s_t aivision_get_object(uint8_t port, uint32_t object_index) { - aivision_object_s_t result = AIVISION_OBJECT_ERR_INIT; + aivision_object_s_t result = AIVISION_OBJECT_ERR_INIT; if (!claim_port_try(port - 1, E_DEVICE_AIVISION)) { return result; } v5_smart_device_s_t* device = registry_get_device(port - 1); - vexDeviceAiVisionObjectGet(device->device_info, object_index, (V5_DeviceAiVisionObject*)&result); + vexDeviceAiVisionObjectGet( + device->device_info, + object_index, + (V5_DeviceAiVisionObject*)&result + ); return_port(port - 1, result); } aivision_code_s_t aivision_get_code(uint8_t port, uint32_t id) { - aivision_code_s_t code = AIVISION_CODE_ERR_INIT; - if (!claim_port_try(port - 1, E_DEVICE_AIVISION)) { - return code; - } - v5_smart_device_s_t* device = registry_get_device(port - 1); - V5_DeviceAiVisionCode _code; - vexDeviceAiVisionCodeGet(device->device_info, id, &_code); - code.id = _code.id; - code.length = _code.len; - code.c1 = _code.c1; - code.c2 = _code.c2; - code.c3 = _code.c3; - code.c4 = _code.c4; - code.c5 = _code.c5; - return_port(port - 1, code); + aivision_code_s_t code = AIVISION_CODE_ERR_INIT; + if (!claim_port_try(port - 1, E_DEVICE_AIVISION)) { + return code; + } + v5_smart_device_s_t* device = registry_get_device(port - 1); + V5_DeviceAiVisionCode _code; + vexDeviceAiVisionCodeGet(device->device_info, id, &_code); + code.id = _code.id; + code.length = _code.len; + code.c1 = _code.c1; + code.c2 = _code.c2; + code.c3 = _code.c3; + code.c4 = _code.c4; + code.c5 = _code.c5; + return_port(port - 1, code); } int32_t aivision_set_code(uint8_t port, aivision_code_s_t* code) { - claim_port_i(port - 1, E_DEVICE_AIVISION); - V5_DeviceAiVisionCode _code; - _code.id = code->id; - _code.len = code->length; - _code.c1 = code->c1; - _code.c2 = code->c2; - _code.c3 = code->c3; - _code.c4 = code->c4; - _code.c5 = code->c5; - vexDeviceAiVisionCodeSet(device->device_info, &_code); - return_port(port - 1, PROS_SUCCESS); + claim_port_i(port - 1, E_DEVICE_AIVISION); + V5_DeviceAiVisionCode _code; + _code.id = code->id; + _code.len = code->length; + _code.c1 = code->c1; + _code.c2 = code->c2; + _code.c3 = code->c3; + _code.c4 = code->c4; + _code.c5 = code->c5; + vexDeviceAiVisionCodeSet(device->device_info, &_code); + return_port(port - 1, PROS_SUCCESS); } diff --git a/src/devices/vdml_ai_vision.cpp b/src/devices/vdml_ai_vision.cpp index 2948ec3c..58136aee 100644 --- a/src/devices/vdml_ai_vision.cpp +++ b/src/devices/vdml_ai_vision.cpp @@ -12,93 +12,94 @@ #include "pros/ai_vision.hpp" #include "pros/device.h" -#include "vdml/vdml.h" +#include "vdml/vdml.hpp" namespace pros { inline namespace v5 { using namespace pros::c; AivisionModeType operator|(AivisionModeType lhs, AivisionModeType rhs) { - return static_cast(static_cast(lhs) | static_cast(rhs)); + return static_cast(static_cast(lhs) | static_cast(rhs)); } -AIVision::AIVision(const uint8_t port) : Device(port, DeviceType::aivision) { - // empty constructor +AIVision::AIVision(const uint8_t port) + : Device(port, DeviceType::aivision) { + // empty constructor } std::vector AIVision::get_all_devices() { - std::vector matching_devices{Device::get_all_devices(DeviceType::aivision)}; + std::vector matching_devices{Device::get_all_devices(DeviceType::aivision)}; - std::vector return_vector; - for (const auto& device : matching_devices) { - return_vector.push_back(device); - } - return return_vector; + std::vector return_vector; + for (const auto& device : matching_devices) { + return_vector.push_back(device); + } + return return_vector; } bool AIVision::is_type(AivisionDetectType type, const aivision_object_s_t& object) { - return static_cast(type) == object.type; + return static_cast(type) == object.type; } int32_t AIVision::reset() { - return c::aivision_reset(this->_port); + return c::aivision_reset(this->_port); } int32_t AIVision::get_enabled_detection_types() { - return c::aivision_get_enabled_detection_types(this->_port); + return c::aivision_get_enabled_detection_types(this->_port); } int32_t AIVision::enable_detection_types(AivisionModeType types_mask) { - return c::aivision_enable_detection_types(this->_port, static_cast(types_mask)); + return c::aivision_enable_detection_types(this->_port, static_cast(types_mask)); } int32_t AIVision::disable_detection_types(AivisionModeType types_mask) { - return c::aivision_disable_detection_types(this->_port, static_cast(types_mask)); + return c::aivision_disable_detection_types(this->_port, static_cast(types_mask)); } int32_t AIVision::set_tag_family(AivisionTagFamily family) { - return c::aivision_set_tag_family(this->_port, static_cast(family)); + return c::aivision_set_tag_family(this->_port, static_cast(family)); } int32_t AIVision::set_color(pros::aivision_color_s_t& color) { - return c::aivision_set_color(this->_port, &color); + return c::aivision_set_color(this->_port, &color); } aivision_color_s_t AIVision::get_color(uint32_t id) { - return c::aivision_get_color(this->_port, id); + return c::aivision_get_color(this->_port, id); } int32_t AIVision::get_object_count() { - return c::aivision_get_object_count(this->_port); + return c::aivision_get_object_count(this->_port); } aivision_object_s_t AIVision::get_object(uint32_t object_index) { - return c::aivision_get_object(this->_port, object_index); + return c::aivision_get_object(this->_port, object_index); } std::vector AIVision::get_all_objects() { - int32_t count = this->get_object_count(); - if (count < 0 || count == PROS_ERR) { - return {}; - } - std::vector objects = std::vector(count); - for (size_t idx = 0; idx < count; idx++) { - objects.emplace_back(this->get_object(idx)); - } - return objects; + int32_t count = this->get_object_count(); + if (count < 0 || count == PROS_ERR) { + return {}; + } + std::vector objects = std::vector(count); + for (size_t idx = 0; idx < count; idx++) { + objects.emplace_back(this->get_object(idx)); + } + return objects; } uint32_t AIVision::get_class_name(int32_t id, uint8_t* class_name) { - return c::aivision_get_class_name(this->_port, id, class_name); + return c::aivision_get_class_name(this->_port, id, class_name); } aivision_code_s_t AIVision::get_code(uint32_t id) { - return c::aivision_get_code(this->_port, id); + return c::aivision_get_code(this->_port, id); } uint32_t AIVision::set_code(pros::aivision_code_s_t& code) { - return c::aivision_set_code(this->_port, &code); + return c::aivision_set_code(this->_port, &code); } -} // namespace v5 -} // namespace pros \ No newline at end of file +} // namespace v5 +} // namespace pros \ No newline at end of file diff --git a/src/devices/vdml_device.c b/src/devices/vdml_device.c index fc4b69eb..e39cf892 100644 --- a/src/devices/vdml_device.c +++ b/src/devices/vdml_device.c @@ -16,14 +16,14 @@ */ #include "pros/device.h" -#include "vdml/vdml.h" +#include "vdml/vdml.hpp" v5_device_e_t get_plugged_type(uint8_t port) { - if (!port_mutex_take(port - 1)) { - errno = EACCES; - return E_DEVICE_UNDEFINED; - } - v5_device_e_t type = registry_get_plugged_type(port - 1); - - return_port(port - 1, type); + if (!port_mutex_take(port - 1)) { + errno = EACCES; + return E_DEVICE_UNDEFINED; + } + v5_device_e_t type = registry_get_plugged_type(port - 1); + + return_port(port - 1, type); } diff --git a/src/devices/vdml_device.cpp b/src/devices/vdml_device.cpp index a3e4bf00..38ad0d82 100644 --- a/src/devices/vdml_device.cpp +++ b/src/devices/vdml_device.cpp @@ -16,7 +16,7 @@ */ #include "pros/device.hpp" -#include "vdml/vdml.h" +#include "vdml/vdml.hpp" namespace pros { inline namespace v5 { @@ -24,48 +24,50 @@ inline namespace v5 { bool Device::is_installed() { std::uint8_t zero_indexed_port = _port - 1; port_mutex_take(zero_indexed_port); - pros::DeviceType plugged_device_type = (pros::DeviceType)pros::c::registry_get_plugged_type(zero_indexed_port); - return_port(zero_indexed_port, _deviceType == plugged_device_type); + pros::DeviceType plugged_device_type = + (pros::DeviceType)pros::c::registry_get_plugged_type(zero_indexed_port); + return_port(zero_indexed_port, _deviceType == plugged_device_type); } std::uint8_t Device::get_port(void) const { - return _port; + return _port; } pros::DeviceType Device::get_plugged_type() const { - return(get_plugged_type(_port)); + 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); - - return_port(port - 1, type); + if (!port_mutex_take(port - 1)) { + errno = EACCES; + return DeviceType::undefined; + } + DeviceType type = (DeviceType)pros::c::registry_get_plugged_type(port - 1); + + return_port(port - 1, type); } std::vector Device::get_all_devices(pros::DeviceType device_type) { - std::vector device_list {}; + 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; - } + 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; + 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) {} - +Device::Device(const std::uint8_t port) + : _port(port) {} -} // namespace v5 -} // namespace pros +} // namespace v5 +} // namespace pros diff --git a/src/devices/vdml_distance.c b/src/devices/vdml_distance.c index 1a6a7f7e..2335cbf6 100644 --- a/src/devices/vdml_distance.c +++ b/src/devices/vdml_distance.c @@ -10,44 +10,44 @@ * file, You can obtain one at http://mozilla.org/MPL/2.0/. */ -#include - #include "pros/distance.h" #include "v5_api.h" -#include "vdml/registry.h" -#include "vdml/vdml.h" +#include "vdml/registry.hpp" +#include "vdml/vdml.hpp" + +#include -#define ERROR_DISTANCE_BAD_PORT(device, err_return) \ - if (!(vexDeviceDistanceStatusGet(device->device_info) == 0x82 || \ - vexDeviceDistanceStatusGet(device->device_info) == 0x86)) { \ - errno = EAGAIN; \ - return_port(port - 1, err_return); \ - } +#define ERROR_DISTANCE_BAD_PORT(device, err_return) \ + if (!(vexDeviceDistanceStatusGet(device->device_info) == 0x82 \ + || vexDeviceDistanceStatusGet(device->device_info) == 0x86)) { \ + errno = EAGAIN; \ + return_port(port - 1, err_return); \ + } int32_t distance_get(uint8_t port) { - claim_port_i(port - 1, E_DEVICE_DISTANCE); - ERROR_DISTANCE_BAD_PORT(device, PROS_ERR); - int32_t rtn = vexDeviceDistanceDistanceGet(device->device_info); - return_port(port - 1, rtn); + claim_port_i(port - 1, E_DEVICE_DISTANCE); + ERROR_DISTANCE_BAD_PORT(device, PROS_ERR); + int32_t rtn = vexDeviceDistanceDistanceGet(device->device_info); + return_port(port - 1, rtn); } int32_t distance_get_confidence(uint8_t port) { - claim_port_i(port - 1, E_DEVICE_DISTANCE); - ERROR_DISTANCE_BAD_PORT(device, PROS_ERR); - int32_t rtn = vexDeviceDistanceConfidenceGet(device->device_info); - return_port(port - 1, rtn); + claim_port_i(port - 1, E_DEVICE_DISTANCE); + ERROR_DISTANCE_BAD_PORT(device, PROS_ERR); + int32_t rtn = vexDeviceDistanceConfidenceGet(device->device_info); + return_port(port - 1, rtn); } int32_t distance_get_object_size(uint8_t port) { - claim_port_i(port - 1, E_DEVICE_DISTANCE); - ERROR_DISTANCE_BAD_PORT(device, PROS_ERR); - int32_t rtn = vexDeviceDistanceObjectSizeGet(device->device_info); - return_port(port - 1, rtn); + claim_port_i(port - 1, E_DEVICE_DISTANCE); + ERROR_DISTANCE_BAD_PORT(device, PROS_ERR); + int32_t rtn = vexDeviceDistanceObjectSizeGet(device->device_info); + return_port(port - 1, rtn); } double distance_get_object_velocity(uint8_t port) { - claim_port_i(port - 1, E_DEVICE_DISTANCE); - ERROR_DISTANCE_BAD_PORT(device, PROS_ERR); - double rtn = vexDeviceDistanceObjectVelocityGet(device->device_info); - return_port(port - 1, rtn); + claim_port_i(port - 1, E_DEVICE_DISTANCE); + ERROR_DISTANCE_BAD_PORT(device, PROS_ERR); + double rtn = vexDeviceDistanceObjectVelocityGet(device->device_info); + return_port(port - 1, rtn); } diff --git a/src/devices/vdml_distance.cpp b/src/devices/vdml_distance.cpp index 2a39ebe1..751d1ab1 100644 --- a/src/devices/vdml_distance.cpp +++ b/src/devices/vdml_distance.cpp @@ -11,52 +11,52 @@ */ #include "pros/distance.hpp" -#include "vdml/vdml.h" +#include "vdml/vdml.hpp" namespace pros { inline namespace v5 { -Distance::Distance(const std::uint8_t port) : Device(port, DeviceType::distance) {} +Distance::Distance(const std::uint8_t port) + : Device(port, DeviceType::distance) {} std::int32_t Distance::get() { - return pros::c::distance_get(_port); + return pros::c::distance_get(_port); } std::int32_t Distance::get_distance() { - return get(); + 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::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); + return pros::c::distance_get_confidence(_port); } std::int32_t Distance::get_object_size() { - return pros::c::distance_get_object_size(_port); + return pros::c::distance_get_object_size(_port); } double Distance::get_object_velocity() { - return pros::c::distance_get_object_velocity(_port); + return pros::c::distance_get_object_velocity(_port); } - std::ostream& operator<<(std::ostream& os, pros::Distance& distance) { - os << "Distance ["; - os << "port: " << distance.get_port(); - os << ", distance: " << distance.get(); - os << ", confidence: " << distance.get_confidence(); - os << ", object size: " << distance.get_object_size(); - os << ", object velocity: " << distance.get_object_velocity(); - os << "]"; - return os; + os << "Distance ["; + os << "port: " << distance.get_port(); + os << ", distance: " << distance.get(); + os << ", confidence: " << distance.get_confidence(); + os << ", object size: " << distance.get_object_size(); + os << ", object velocity: " << distance.get_object_velocity(); + os << "]"; + return os; } namespace literals { @@ -65,4 +65,4 @@ const pros::Distance operator"" _dist(const unsigned long long int d) { } } // namespace literals } // namespace v5 -} // namespace pros +} // namespace pros diff --git a/src/devices/vdml_ext_adi.c b/src/devices/vdml_ext_adi.c index f7695cf1..84d13306 100644 --- a/src/devices/vdml_ext_adi.c +++ b/src/devices/vdml_ext_adi.c @@ -10,17 +10,17 @@ * file, You can obtain one at http://mozilla.org/MPL/2.0/. */ -#include -#include -#include - #include "kapi.h" #include "pros/adi.h" #include "pros/ext_adi.h" #include "v5_api.h" #include "vdml/port.h" -#include "vdml/registry.h" -#include "vdml/vdml.h" +#include "vdml/registry.hpp" +#include "vdml/vdml.hpp" + +#include +#include +#include #define ADI_MOTOR_MAX_SPEED 127 #define ADI_MOTOR_MIN_SPEED -128 @@ -32,70 +32,90 @@ #define GYRO_CALIBRATION_TIME 1300 typedef union adi_data { - struct { - int32_t calib; - } analog_data; - struct { - bool was_pressed; - } digital_data; - struct { - bool reversed; - } encoder_data; - struct { - adi_potentiometer_type_e_t potentiometer_type; - } potentiometer_data; - struct __attribute__((packed)) { - double multiplier; - double tare_value; - } gyro_data; + struct { + int32_t calib; + } analog_data; + + struct { + bool was_pressed; + } digital_data; + + struct { + bool reversed; + } encoder_data; + + struct { + adi_potentiometer_type_e_t potentiometer_type; + } potentiometer_data; + + struct __attribute__((packed)) { + double multiplier; + double tare_value; + } 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". +// 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); +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 -#define transform_adi_port(port) \ - if (port >= 'a' && port <= 'h') \ - port -= 'a'; \ - else if (port >= 'A' && port <= 'H') \ - port -= 'A'; \ - else \ - port--; \ - if (port > 7 || port < 0) { \ - errno = ENXIO; \ - 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_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); \ - } - -#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); \ - } +#define transform_adi_port(port) \ + if (port >= 'a' && port <= 'h') \ + port -= 'a'; \ + else if (port >= 'A' && port <= 'H') \ + port -= 'A'; \ + else \ + port--; \ + if (port > 7 || port < 0) { \ + errno = ENXIO; \ + 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_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); \ + } + +#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); \ + } /* * Validates 3 things: @@ -104,408 +124,437 @@ int32_t vexAdiAddrLedSet(uint32_t index, uint32_t port, uint32_t* pData, uint32_ * * Returns PROS_ERR if one of these is false. */ -#define validate_twowire(port_top, port_bottom) \ - if (abs(port_top - port_bottom) > 1) { \ - errno = ENXIO; \ - return PROS_ERR; \ - } \ - int port; \ - if (port_top < port_bottom) \ - port = port_top; \ - else if (port_bottom < port_top) \ - port = port_bottom; \ - else { \ - errno = EINVAL; \ - return PROS_ERR; \ - } \ - if (port % 2 == 1) { \ - errno = EINVAL; \ - return PROS_ERR; \ - } +#define validate_twowire(port_top, port_bottom) \ + if (abs(port_top - port_bottom) > 1) { \ + errno = ENXIO; \ + return PROS_ERR; \ + } \ + int port; \ + if (port_top < port_bottom) \ + port = port_top; \ + else if (port_bottom < port_top) \ + port = port_bottom; \ + else { \ + errno = EINVAL; \ + return PROS_ERR; \ + } \ + if (port % 2 == 1) { \ + errno = EINVAL; \ + return PROS_ERR; \ + } adi_port_config_e_t ext_adi_port_get_config(uint8_t smart_port, uint8_t adi_port) { - transform_adi_port(adi_port); - claim_port_i(smart_port - 1, E_DEVICE_ADI); - adi_port_config_e_t rtn = (adi_port_config_e_t)vexDeviceAdiPortConfigGet(device->device_info, adi_port); - return_port(smart_port - 1, rtn); + transform_adi_port(adi_port); + claim_port_i(smart_port - 1, E_DEVICE_ADI); + adi_port_config_e_t rtn = + (adi_port_config_e_t)vexDeviceAdiPortConfigGet(device->device_info, adi_port); + return_port(smart_port - 1, rtn); } int32_t ext_adi_port_get_value(uint8_t smart_port, uint8_t adi_port) { - transform_adi_port(adi_port); - claim_port_i(smart_port - 1, E_DEVICE_ADI); - int32_t rtn = vexDeviceAdiValueGet(device->device_info, adi_port); - return_port(smart_port - 1, rtn); + transform_adi_port(adi_port); + claim_port_i(smart_port - 1, E_DEVICE_ADI); + int32_t rtn = vexDeviceAdiValueGet(device->device_info, adi_port); + return_port(smart_port - 1, rtn); } int32_t ext_adi_port_set_config(uint8_t smart_port, uint8_t adi_port, adi_port_config_e_t type) { - transform_adi_port(adi_port); - claim_port_i(smart_port - 1, E_DEVICE_ADI); - vexDeviceAdiPortConfigSet(device->device_info, adi_port, (V5_AdiPortConfiguration)type); - return_port(smart_port - 1, 1); + transform_adi_port(adi_port); + claim_port_i(smart_port - 1, E_DEVICE_ADI); + vexDeviceAdiPortConfigSet(device->device_info, adi_port, (V5_AdiPortConfiguration)type); + return_port(smart_port - 1, 1); } int32_t ext_adi_port_set_value(uint8_t smart_port, uint8_t adi_port, int32_t value) { - transform_adi_port(adi_port); - claim_port_i(smart_port - 1, E_DEVICE_ADI); - vexDeviceAdiValueSet(device->device_info, adi_port, value); - return_port(smart_port - 1, 1); + transform_adi_port(adi_port); + claim_port_i(smart_port - 1, E_DEVICE_ADI); + vexDeviceAdiValueSet(device->device_info, adi_port, value); + return_port(smart_port - 1, 1); } int32_t ext_adi_analog_calibrate(uint8_t smart_port, uint8_t adi_port) { - transform_adi_port(adi_port); - claim_port_i(smart_port - 1, E_DEVICE_ADI); - validate_type(device, adi_port, smart_port - 1, E_ADI_ANALOG_IN); - uint32_t total = 0; - for (uint32_t i = 0; i < 512; i++) { - total += vexDeviceAdiValueGet(device->device_info, adi_port); - task_delay(1); // TODO: If smart ports (and the ADI) only update every 10ms, this really only reads 56 samples, - // maybe change to a 10ms - } - adi_data_s_t* const adi_data = &((adi_data_s_t*)(device->pad))[adi_port]; - adi_data->analog_data.calib = (int32_t)((total + 16) >> 5); - return_port(smart_port - 1, (int32_t)((total + 256) >> 9)); + transform_adi_port(adi_port); + claim_port_i(smart_port - 1, E_DEVICE_ADI); + validate_type(device, adi_port, smart_port - 1, E_ADI_ANALOG_IN); + uint32_t total = 0; + for (uint32_t i = 0; i < 512; i++) { + total += vexDeviceAdiValueGet(device->device_info, adi_port); + task_delay(1); // TODO: If smart ports (and the ADI) only update every 10ms, this really + // only reads 56 samples, maybe change to a 10ms + } + adi_data_s_t* const adi_data = &((adi_data_s_t*)(device->pad))[adi_port]; + adi_data->analog_data.calib = (int32_t)((total + 16) >> 5); + return_port(smart_port - 1, (int32_t)((total + 256) >> 9)); } int32_t ext_adi_analog_read(uint8_t smart_port, uint8_t adi_port) { - transform_adi_port(adi_port); - claim_port_i(smart_port - 1, E_DEVICE_ADI); - validate_type(device, adi_port, smart_port - 1, E_ADI_ANALOG_IN); - int32_t rtn = vexDeviceAdiValueGet(device->device_info, adi_port); - return_port(smart_port - 1, rtn); + transform_adi_port(adi_port); + claim_port_i(smart_port - 1, E_DEVICE_ADI); + validate_type(device, adi_port, smart_port - 1, E_ADI_ANALOG_IN); + int32_t rtn = vexDeviceAdiValueGet(device->device_info, adi_port); + return_port(smart_port - 1, rtn); } int32_t ext_adi_analog_read_calibrated(uint8_t smart_port, uint8_t adi_port) { - transform_adi_port(adi_port); - claim_port_i(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]; - int32_t rtn = vexDeviceAdiValueGet(device->device_info, adi_port) - (adi_data->analog_data.calib >> 4); - return_port(smart_port - 1, rtn); + transform_adi_port(adi_port); + claim_port_i(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]; + int32_t rtn = + vexDeviceAdiValueGet(device->device_info, adi_port) - (adi_data->analog_data.calib >> 4); + return_port(smart_port - 1, rtn); } int32_t ext_adi_analog_read_calibrated_HR(uint8_t smart_port, uint8_t adi_port) { - transform_adi_port(adi_port); - claim_port_i(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]; - int32_t rtn = (vexDeviceAdiValueGet(device->device_info, adi_port) << 4) - adi_data->analog_data.calib; - return_port(smart_port - 1, rtn); + transform_adi_port(adi_port); + claim_port_i(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]; + int32_t rtn = + (vexDeviceAdiValueGet(device->device_info, adi_port) << 4) - adi_data->analog_data.calib; + return_port(smart_port - 1, rtn); } int32_t ext_adi_digital_read(uint8_t smart_port, uint8_t adi_port) { - transform_adi_port(adi_port); - claim_port_i(smart_port - 1, E_DEVICE_ADI); - validate_type(device, adi_port, smart_port - 1, E_ADI_DIGITAL_IN); - int32_t rtn = vexDeviceAdiValueGet(device->device_info, adi_port); - return_port(smart_port - 1, rtn); + transform_adi_port(adi_port); + claim_port_i(smart_port - 1, E_DEVICE_ADI); + validate_type(device, adi_port, smart_port - 1, E_ADI_DIGITAL_IN); + int32_t rtn = vexDeviceAdiValueGet(device->device_info, adi_port); + return_port(smart_port - 1, rtn); } int32_t ext_adi_digital_get_new_press(uint8_t smart_port, uint8_t adi_port) { - transform_adi_port(adi_port); - claim_port_i(smart_port - 1, E_DEVICE_ADI); - validate_type(device, adi_port, smart_port - 1, E_ADI_DIGITAL_IN); + transform_adi_port(adi_port); + claim_port_i(smart_port - 1, E_DEVICE_ADI); + validate_type(device, adi_port, smart_port - 1, E_ADI_DIGITAL_IN); - adi_data_s_t* const adi_data = &((adi_data_s_t*)(device->pad))[adi_port]; + adi_data_s_t* const adi_data = &((adi_data_s_t*)(device->pad))[adi_port]; - int32_t pressed = vexDeviceAdiValueGet(device->device_info, adi_port); + int32_t pressed = vexDeviceAdiValueGet(device->device_info, adi_port); - if (!pressed) - adi_data->digital_data.was_pressed = false; - else if (!adi_data->digital_data.was_pressed) { - // Button is currently pressed and was not detected as being pressed during last check - adi_data->digital_data.was_pressed = true; - return_port(smart_port - 1, true); - } + if (!pressed) + adi_data->digital_data.was_pressed = false; + else if (!adi_data->digital_data.was_pressed) { + // Button is currently pressed and was not detected as being pressed during last check + adi_data->digital_data.was_pressed = true; + return_port(smart_port - 1, true); + } - return_port(smart_port - 1, false); + return_port(smart_port - 1, false); } int32_t ext_adi_digital_write(uint8_t smart_port, uint8_t adi_port, bool value) { - transform_adi_port(adi_port); - claim_port_i(smart_port - 1, E_DEVICE_ADI); - validate_type(device, adi_port, smart_port - 1, E_ADI_DIGITAL_OUT); - vexDeviceAdiValueSet(device->device_info, adi_port, (int32_t)value); - return_port(smart_port - 1, 1); + transform_adi_port(adi_port); + claim_port_i(smart_port - 1, E_DEVICE_ADI); + validate_type(device, adi_port, smart_port - 1, E_ADI_DIGITAL_OUT); + vexDeviceAdiValueSet(device->device_info, adi_port, (int32_t)value); + return_port(smart_port - 1, 1); } int32_t ext_adi_pin_mode(uint8_t smart_port, uint8_t adi_port, uint8_t mode) { - switch (mode) { - case INPUT: - ext_adi_port_set_config(smart_port, adi_port, E_ADI_DIGITAL_IN); - break; - case OUTPUT: - ext_adi_port_set_config(smart_port, adi_port, E_ADI_DIGITAL_OUT); - break; - case INPUT_ANALOG: - ext_adi_port_set_config(smart_port, adi_port, E_ADI_ANALOG_IN); - break; - case OUTPUT_ANALOG: - ext_adi_port_set_config(smart_port, adi_port, E_ADI_ANALOG_OUT); - break; - default: - errno = EINVAL; - return PROS_ERR; - }; - return 1; + switch (mode) { + case INPUT: + ext_adi_port_set_config(smart_port, adi_port, E_ADI_DIGITAL_IN); + break; + case OUTPUT: + ext_adi_port_set_config(smart_port, adi_port, E_ADI_DIGITAL_OUT); + break; + case INPUT_ANALOG: + ext_adi_port_set_config(smart_port, adi_port, E_ADI_ANALOG_IN); + break; + case OUTPUT_ANALOG: + ext_adi_port_set_config(smart_port, adi_port, E_ADI_ANALOG_OUT); + break; + default: + errno = EINVAL; + return PROS_ERR; + }; + return 1; } int32_t ext_adi_motor_set(uint8_t smart_port, uint8_t adi_port, int8_t speed) { - transform_adi_port(adi_port); - claim_port_i(smart_port - 1, E_DEVICE_ADI); - validate_motor(device, adi_port, smart_port - 1); - if (speed > ADI_MOTOR_MAX_SPEED) - speed = ADI_MOTOR_MAX_SPEED; - else if (speed < ADI_MOTOR_MIN_SPEED) - speed = ADI_MOTOR_MIN_SPEED; - vexDeviceAdiValueSet(device->device_info, adi_port, speed); - return_port(smart_port - 1, 1); + transform_adi_port(adi_port); + claim_port_i(smart_port - 1, E_DEVICE_ADI); + validate_motor(device, adi_port, smart_port - 1); + if (speed > ADI_MOTOR_MAX_SPEED) + speed = ADI_MOTOR_MAX_SPEED; + else if (speed < ADI_MOTOR_MIN_SPEED) + speed = ADI_MOTOR_MIN_SPEED; + vexDeviceAdiValueSet(device->device_info, adi_port, speed); + return_port(smart_port - 1, 1); } int32_t ext_adi_motor_get(uint8_t smart_port, uint8_t adi_port) { - transform_adi_port(adi_port); - claim_port_i(smart_port - 1, E_DEVICE_ADI); - validate_motor(device, adi_port, smart_port - 1); - int32_t rtn = vexDeviceAdiValueGet(device->device_info, adi_port) - ADI_MOTOR_MAX_SPEED; - return_port(smart_port - 1, rtn); + transform_adi_port(adi_port); + claim_port_i(smart_port - 1, E_DEVICE_ADI); + validate_motor(device, adi_port, smart_port - 1); + int32_t rtn = vexDeviceAdiValueGet(device->device_info, adi_port) - ADI_MOTOR_MAX_SPEED; + return_port(smart_port - 1, rtn); } int32_t ext_adi_motor_stop(uint8_t smart_port, uint8_t adi_port) { - return ext_adi_motor_set(smart_port, adi_port, 0); + return ext_adi_motor_set(smart_port, adi_port, 0); } -ext_adi_encoder_t ext_adi_encoder_init(uint8_t smart_port, uint8_t adi_port_top, uint8_t adi_port_bottom, - bool reverse) { - transform_adi_port(adi_port_top); - transform_adi_port(adi_port_bottom); - validate_twowire(adi_port_top, adi_port_bottom); - claim_port_i(smart_port - 1, E_DEVICE_ADI); +ext_adi_encoder_t ext_adi_encoder_init( + uint8_t smart_port, + uint8_t adi_port_top, + uint8_t adi_port_bottom, + bool reverse +) { + transform_adi_port(adi_port_top); + transform_adi_port(adi_port_bottom); + validate_twowire(adi_port_top, adi_port_bottom); + claim_port_i(smart_port - 1, E_DEVICE_ADI); - 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, port + 1)); + 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, 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 - 1, E_DEVICE_ADI); - validate_type(device, adi_port, smart_port - 1, E_ADI_LEGACY_ENCODER); + uint8_t smart_port, adi_port; + get_ports(enc, smart_port, adi_port); + transform_adi_port(adi_port); + claim_port_i(smart_port - 1, E_DEVICE_ADI); + validate_type(device, adi_port, smart_port - 1, E_ADI_LEGACY_ENCODER); - int32_t rtn; - adi_data_s_t* const adi_data = &((adi_data_s_t*)(device->pad))[adi_port]; - if (adi_data->encoder_data.reversed) - rtn = -vexDeviceAdiValueGet(device->device_info, adi_port); - else - rtn = vexDeviceAdiValueGet(device->device_info, adi_port); - return_port(smart_port - 1, rtn); + int32_t rtn; + adi_data_s_t* const adi_data = &((adi_data_s_t*)(device->pad))[adi_port]; + if (adi_data->encoder_data.reversed) + rtn = -vexDeviceAdiValueGet(device->device_info, adi_port); + else + rtn = vexDeviceAdiValueGet(device->device_info, adi_port); + 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 - 1, E_DEVICE_ADI); - validate_type(device, adi_port, smart_port - 1, E_ADI_LEGACY_ENCODER); + uint8_t smart_port, adi_port; + get_ports(enc, smart_port, adi_port); + transform_adi_port(adi_port); + 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, 1); + vexDeviceAdiValueSet(device->device_info, adi_port, 0); + 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 - 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, 1); -} - -ext_adi_ultrasonic_t ext_adi_ultrasonic_init(uint8_t smart_port, uint8_t adi_port_ping, uint8_t adi_port_echo) { - transform_adi_port(adi_port_ping); - transform_adi_port(adi_port_echo); - validate_twowire(adi_port_ping, adi_port_echo); - if (port != adi_port_ping) { - 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, port + 1)); + uint8_t smart_port, adi_port; + get_ports(enc, smart_port, adi_port); + transform_adi_port(adi_port); + 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, 1); +} + +ext_adi_ultrasonic_t +ext_adi_ultrasonic_init(uint8_t smart_port, uint8_t adi_port_ping, uint8_t adi_port_echo) { + transform_adi_port(adi_port_ping); + transform_adi_port(adi_port_echo); + validate_twowire(adi_port_ping, adi_port_echo); + if (port != adi_port_ping) { + 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, 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 - 1, E_DEVICE_ADI); - validate_type(device, adi_port, smart_port - 1, E_ADI_LEGACY_ULTRASONIC); + uint8_t smart_port, adi_port; + get_ports(ult, smart_port, adi_port); + transform_adi_port(adi_port); + 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 - 1, rtn); + int32_t rtn = vexDeviceAdiValueGet(device->device_info, adi_port); + 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 - 1, E_DEVICE_ADI); - validate_type(device, adi_port, smart_port - 1, E_ADI_LEGACY_ULTRASONIC); + uint8_t smart_port, adi_port; + get_ports(ult, smart_port, adi_port); + transform_adi_port(adi_port); + 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, 1); + vexDeviceAdiPortConfigSet(device->device_info, adi_port, E_ADI_TYPE_UNDEFINED); + return_port(smart_port - 1, 1); } ext_adi_gyro_t ext_adi_gyro_init(uint8_t smart_port, uint8_t adi_port, double multiplier) { - transform_adi_port(adi_port); - claim_port_i(smart_port - 1, E_DEVICE_ADI); - - if (multiplier == 0) multiplier = 1; - adi_data_s_t* const adi_data = &((adi_data_s_t*)(device->pad))[adi_port]; - adi_data->gyro_data.multiplier = multiplier; - adi_data->gyro_data.tare_value = 0; - - 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, adi_port + 1)); - } - - vexDeviceAdiPortConfigSet(device->device_info, adi_port, E_ADI_LEGACY_GYRO); - if (xTaskGetSchedulerState() == taskSCHEDULER_RUNNING) { - // If the scheduler is currently running (meaning that this is not called - // from a global constructor, for example) then delay for the duration of - // the calibration time in VexOS. - delay(GYRO_CALIBRATION_TIME); - } - return_port(smart_port - 1, merge_adi_ports(smart_port, adi_port + 1)); + transform_adi_port(adi_port); + claim_port_i(smart_port - 1, E_DEVICE_ADI); + + if (multiplier == 0) + multiplier = 1; + adi_data_s_t* const adi_data = &((adi_data_s_t*)(device->pad))[adi_port]; + adi_data->gyro_data.multiplier = multiplier; + adi_data->gyro_data.tare_value = 0; + + 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, adi_port + 1)); + } + + vexDeviceAdiPortConfigSet(device->device_info, adi_port, E_ADI_LEGACY_GYRO); + if (xTaskGetSchedulerState() == taskSCHEDULER_RUNNING) { + // If the scheduler is currently running (meaning that this is not called + // from a global constructor, for example) then delay for the duration of + // the calibration time in VexOS. + delay(GYRO_CALIBRATION_TIME); + } + 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 - 1, E_DEVICE_ADI); - validate_type_f(device, adi_port, smart_port - 1, E_ADI_LEGACY_GYRO); + uint8_t smart_port, adi_port; + get_ports(gyro, smart_port, adi_port); + transform_adi_port(adi_port); + 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 - 1, rtv); + 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 - 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 - 1, E_DEVICE_ADI); - validate_type(device, adi_port, smart_port - 1, E_ADI_LEGACY_GYRO); + uint8_t smart_port, adi_port; + get_ports(gyro, smart_port, adi_port); + transform_adi_port(adi_port); + 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, 1); + 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, 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 - 1, E_DEVICE_ADI); - validate_type(device, adi_port, smart_port - 1, E_ADI_LEGACY_GYRO); + uint8_t smart_port, adi_port; + get_ports(gyro, smart_port, adi_port); + transform_adi_port(adi_port); + 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, 1); + vexDeviceAdiPortConfigSet(device->device_info, adi_port, E_ADI_TYPE_UNDEFINED); + 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, adi_port + 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, adi_port + 1)); } double ext_adi_potentiometer_get_angle(ext_adi_potentiometer_t potentiometer) { - double rtn; - uint8_t smart_port, adi_port; - get_ports(potentiometer, smart_port, adi_port); - transform_adi_port(adi_port); - 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]; - - switch (adi_data->potentiometer_data.potentiometer_type) { - case E_ADI_POT_EDR: - rtn = vexDeviceAdiValueGet(device->device_info, adi_port) * 250 / 4095.0; - break; - case E_ADI_POT_V2: - rtn = vexDeviceAdiValueGet(device->device_info, adi_port) * 330 / 4095.0; - break; - default: - errno = ENXIO; - rtn = PROS_ERR_F; - } - return_port(smart_port - 1, rtn); + double rtn; + uint8_t smart_port, adi_port; + get_ports(potentiometer, smart_port, adi_port); + transform_adi_port(adi_port); + 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]; + + switch (adi_data->potentiometer_data.potentiometer_type) { + case E_ADI_POT_EDR: + rtn = vexDeviceAdiValueGet(device->device_info, adi_port) * 250 / 4095.0; + break; + case E_ADI_POT_V2: + rtn = vexDeviceAdiValueGet(device->device_info, adi_port) * 330 / 4095.0; + break; + default: + errno = ENXIO; + rtn = PROS_ERR_F; + } + 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, adi_port + 1)); + 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, 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 - 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) { - errno = EINVAL; - 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 - 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) { - uint8_t smart_port, adi_port; - get_ports(led, smart_port, adi_port); - claim_port_i(smart_port - 1, E_DEVICE_ADI); - transform_adi_port(adi_port); - 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 - 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); -} - -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++) { - buffer[i] = color; - } - return ext_adi_led_set(led, buffer, 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 - 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) { + errno = EINVAL; + 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 - 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 +) { + uint8_t smart_port, adi_port; + get_ports(led, smart_port, adi_port); + claim_port_i(smart_port - 1, E_DEVICE_ADI); + transform_adi_port(adi_port); + 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 - 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); +} + +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++) { + buffer[i] = color; + } + return ext_adi_led_set(led, buffer, buffer_length); } int32_t ext_adi_led_clear_all(ext_adi_led_t led, uint32_t* buffer, uint32_t buffer_length) { - return ext_adi_led_set_all(led, buffer, buffer_length, 0); + return ext_adi_led_set_all(led, buffer, buffer_length, 0); } -int32_t ext_adi_led_clear_pixel(ext_adi_led_t led, uint32_t* buffer, uint32_t buffer_length, uint32_t pixel_position) { - return ext_adi_led_set_pixel(led, buffer, buffer_length, 0, pixel_position); +int32_t ext_adi_led_clear_pixel( + ext_adi_led_t led, + uint32_t* buffer, + uint32_t buffer_length, + uint32_t pixel_position +) { + return ext_adi_led_set_pixel(led, buffer, buffer_length, 0, pixel_position); } diff --git a/src/devices/vdml_gps.c b/src/devices/vdml_gps.c index f555ea7a..7a02884f 100644 --- a/src/devices/vdml_gps.c +++ b/src/devices/vdml_gps.c @@ -10,244 +10,249 @@ * 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" +#include "vdml/registry.hpp" +#include "vdml/vdml.hpp" + +#include #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_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 } +#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, PROS_SUCCESS); +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, PROS_SUCCESS); } 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, PROS_SUCCESS); + claim_port_i(port - 1, E_DEVICE_GPS); + vexDeviceGpsOriginSet(device->device_info, xOffset, yOffset); + return_port(port - 1, PROS_SUCCESS); } gps_position_s_t gps_get_offset(uint8_t port) { - gps_position_s_t rtv = {PROS_ERR_F, PROS_ERR_F}; - if (!claim_port_try(port - 1, E_DEVICE_GPS)) { - return rtv; - } - // This is necessary for warning suppression as a packed struct's address - // may be misaligned. - double x = PROS_ERR_F; - double y = PROS_ERR_F; - v5_smart_device_s_t* device = registry_get_device(port - 1); - vexDeviceGpsOriginGet(device->device_info, &x, &y); - rtv.x = x; - rtv.y = y; - return_port(port - 1, rtv); + gps_position_s_t rtv = {PROS_ERR_F, PROS_ERR_F}; + if (!claim_port_try(port - 1, E_DEVICE_GPS)) { + return rtv; + } + // This is necessary for warning suppression as a packed struct's address + // may be misaligned. + double x = PROS_ERR_F; + double y = PROS_ERR_F; + v5_smart_device_s_t* device = registry_get_device(port - 1); + vexDeviceGpsOriginGet(device->device_info, &x, &y); + rtv.x = x; + rtv.y = y; + return_port(port - 1, rtv); } 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, PROS_SUCCESS); + claim_port_i(port - 1, E_DEVICE_GPS); + vexDeviceGpsInitialPositionSet(device->device_info, xInitial, yInitial, headingInitial); + return_port(port - 1, PROS_SUCCESS); } int32_t gps_set_data_rate(uint8_t port, uint32_t rate) { - claim_port_i(port - 1, E_DEVICE_GPS); + 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; - } + // 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, PROS_SUCCESS); + vexDeviceGpsDataRateSet(device->device_info, rate); + return_port(port - 1, PROS_SUCCESS); } 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); + 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_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; - } - 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); + 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); } gps_position_s_t gps_get_position(uint8_t port) { - gps_position_s_t rtv = {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.x = data.position_x; - rtv.y = data.position_y; - return_port(port - 1, rtv); + gps_position_s_t rtv = {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.x = data.position_x; + rtv.y = data.position_y; + return_port(port - 1, rtv); } 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); + V5_DeviceGpsAttitude data; + vexDeviceGpsAttitudeGet(device->device_info, &data, false); + double rtv = data.position_x; + return_port(port - 1, rtv); } 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); + V5_DeviceGpsAttitude data; + vexDeviceGpsAttitudeGet(device->device_info, &data, false); + double rtv = data.position_y; + return_port(port - 1, rtv); } 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); + 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); } 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); + V5_DeviceGpsAttitude data; + vexDeviceGpsAttitudeGet(device->device_info, &data, false); + double rtv = data.pitch; + return_port(port - 1, rtv); } 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); + 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); + 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); + 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); + 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) { - 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_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); } 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); + 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); + 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); + 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)) { - 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); + 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); } 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); + 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); + 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); + 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 ddca1959..813ec7e2 100644 --- a/src/devices/vdml_gps.cpp +++ b/src/devices/vdml_gps.cpp @@ -12,52 +12,63 @@ #include "pros/device.h" #include "pros/gps.hpp" -#include "vdml/vdml.h" +#include "vdml/vdml.hpp" namespace pros { inline namespace v5 { -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::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); + return pros::c::gps_set_offset(_port, xOffset, yOffset); } pros::gps_position_s_t Gps::get_offset() const { - return pros::c::gps_get_offset(_port); + return pros::c::gps_get_offset(_port); } std::int32_t Gps::set_position(double xInitial, double yInitial, double headingInitial) const { - return pros::c::gps_set_position(_port, xInitial, yInitial, headingInitial); + 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); + 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; + 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); + return pros::c::gps_get_error(_port); } pros::gps_status_s_t Gps::get_position_and_orientation() const { - return pros::c::gps_get_position_and_orientation(_port); + 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); + return pros::c::gps_get_position(_port); } double Gps::get_position_x() const { @@ -85,15 +96,15 @@ double Gps::get_yaw() const { } double Gps::get_heading() const { - return pros::c::gps_get_heading(_port); + return pros::c::gps_get_heading(_port); } double Gps::get_heading_raw() const { - return pros::c::gps_get_heading_raw(_port); + return pros::c::gps_get_heading_raw(_port); } pros::gps_gyro_s_t Gps::get_gyro_rate() const { - return pros::c::gps_get_gyro_rate(_port); + return pros::c::gps_get_gyro_rate(_port); } double Gps::get_gyro_rate_x() const { @@ -101,15 +112,15 @@ double Gps::get_gyro_rate_x() const { } double Gps::get_gyro_rate_y() const { - return pros::c::gps_get_gyro_rate_y(_port); + return pros::c::gps_get_gyro_rate_y(_port); } double Gps::get_gyro_rate_z() const { - return pros::c::gps_get_gyro_rate_z(_port); + 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); + return pros::c::gps_get_accel(_port); } double Gps::get_accel_x() const { @@ -117,43 +128,43 @@ double Gps::get_accel_x() const { } double Gps::get_accel_y() const { - return pros::c::gps_get_accel_y(_port); + return pros::c::gps_get_accel_y(_port); } double Gps::get_accel_z() const { - return pros::c::gps_get_accel_z(_port); + 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_position_and_orientation(); - os << "Gps ["; - os << "port: " << gps._port; - os << ", x: " << data.x; - os << ", y: " << data.y; - os << ", heading: " << gps.get_heading(); - os << "]"; - return os; + 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 << "]"; + 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); + 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); + return pros::Gps(g); } -} // namespace literals -} // namespace v5 -} // namespace pros +} // namespace literals +} // namespace v5 +} // namespace pros diff --git a/src/devices/vdml_imu.c b/src/devices/vdml_imu.c index 8fb057a1..4539d412 100644 --- a/src/devices/vdml_imu.c +++ b/src/devices/vdml_imu.c @@ -10,406 +10,420 @@ * file, You can obtain one at http://mozilla.org/MPL/2.0/. */ -#include - #include "pros/imu.h" #include "v5_api.h" -#include "vdml/registry.h" -#include "vdml/vdml.h" +#include "vdml/registry.hpp" +#include "vdml/vdml.hpp" + +#include #define IMU_EULER_LIMIT 180 #define IMU_HEADING_MAX 360 #define DEGTORAD (M_PI / 180) -#define ERROR_IMU_STILL_CALIBRATING(port, device, err_return) \ - if (vexDeviceImuStatusGet(device->device_info) & E_IMU_STATUS_CALIBRATING) { \ - errno = EAGAIN; \ - return_port(port - 1, err_return); \ - } +#define ERROR_IMU_STILL_CALIBRATING(port, device, err_return) \ + if (vexDeviceImuStatusGet(device->device_info) & E_IMU_STATUS_CALIBRATING) { \ + errno = EAGAIN; \ + return_port(port - 1, err_return); \ + } #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 { - double heading_offset; - double rotation_offset; - double pitch_offset; - double yaw_offset; - double roll_offset; + double heading_offset; + double rotation_offset; + double pitch_offset; + double yaw_offset; + double roll_offset; } imu_data_s_t; int32_t imu_reset(uint8_t port) { - claim_port_i(port - 1, E_DEVICE_IMU); - ERROR_IMU_STILL_CALIBRATING(port, device, PROS_ERR); - vexDeviceImuReset(device->device_info); - // delay for vexos to set calibration flag, background processing must be called for flag - // to be set. - uint16_t timeoutCount = 0; - // releasing mutex so vexBackgrounProcessing can run without being blocked. - do { - port_mutex_give(port - 1); - task_delay(5); - timeoutCount += 5; - claim_port_i(port - 1, E_DEVICE_IMU); - if (timeoutCount >= IMU_RESET_FLAG_SET_TIMEOUT) { - port_mutex_give(port - 1); - errno = EAGAIN; - return PROS_ERR; - } - device = device; // suppressing compiler warning - } while (!(vexDeviceImuStatusGet(device->device_info) & E_IMU_STATUS_CALIBRATING)); - port_mutex_give(port - 1); - return 1; + claim_port_i(port - 1, E_DEVICE_IMU); + ERROR_IMU_STILL_CALIBRATING(port, device, PROS_ERR); + vexDeviceImuReset(device->device_info); + // delay for vexos to set calibration flag, background processing must be called for flag + // to be set. + uint16_t timeoutCount = 0; + // releasing mutex so vexBackgrounProcessing can run without being blocked. + do { + port_mutex_give(port - 1); + task_delay(5); + timeoutCount += 5; + claim_port_i(port - 1, E_DEVICE_IMU); + if (timeoutCount >= IMU_RESET_FLAG_SET_TIMEOUT) { + port_mutex_give(port - 1); + errno = EAGAIN; + return PROS_ERR; + } + device = device; // suppressing compiler warning + } while (!(vexDeviceImuStatusGet(device->device_info) & E_IMU_STATUS_CALIBRATING)); + port_mutex_give(port - 1); + return 1; } int32_t imu_reset_blocking(uint8_t port) { - claim_port_i(port - 1, E_DEVICE_IMU); - ERROR_IMU_STILL_CALIBRATING(port, device, PROS_ERR); - vexDeviceImuReset(device->device_info); - // delay for vexos to set calibration flag, background processing must be called for flag - // to be set. - uint16_t timeoutCount = 0; - // releasing mutex so vexTasksRun can run without being blocked. - do { - port_mutex_give(port - 1); - task_delay(5); - timeoutCount += 5; - claim_port_i(port - 1, E_DEVICE_IMU); - if (timeoutCount >= IMU_RESET_FLAG_SET_TIMEOUT) { - port_mutex_give(port - 1); - errno = EAGAIN; - return PROS_ERR; - } - 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 { - port_mutex_give(port - 1); - task_delay(5); - timeoutCount += 5; - claim_port_i(port - 1, E_DEVICE_IMU); - if (timeoutCount >= IMU_RESET_TIMEOUT) { - port_mutex_give(port - 1); - errno = EAGAIN; - return PROS_ERR; - } - device = device; // suppressing compiler warning - } while (vexDeviceImuStatusGet(device->device_info) & E_IMU_STATUS_CALIBRATING); - port_mutex_give(port - 1); - return 1; + claim_port_i(port - 1, E_DEVICE_IMU); + ERROR_IMU_STILL_CALIBRATING(port, device, PROS_ERR); + vexDeviceImuReset(device->device_info); + // delay for vexos to set calibration flag, background processing must be called for flag + // to be set. + uint16_t timeoutCount = 0; + // releasing mutex so vexTasksRun can run without being blocked. + do { + port_mutex_give(port - 1); + task_delay(5); + timeoutCount += 5; + claim_port_i(port - 1, E_DEVICE_IMU); + if (timeoutCount >= IMU_RESET_FLAG_SET_TIMEOUT) { + port_mutex_give(port - 1); + errno = EAGAIN; + return PROS_ERR; + } + 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 { + port_mutex_give(port - 1); + task_delay(5); + timeoutCount += 5; + claim_port_i(port - 1, E_DEVICE_IMU); + if (timeoutCount >= IMU_RESET_TIMEOUT) { + port_mutex_give(port - 1); + errno = EAGAIN; + return PROS_ERR; + } + device = device; // suppressing compiler warning + } while (vexDeviceImuStatusGet(device->device_info) & E_IMU_STATUS_CALIBRATING); + port_mutex_give(port - 1); + return 1; } int32_t imu_set_data_rate(uint8_t port, uint32_t rate) { - claim_port_i(port - 1, E_DEVICE_IMU); - ERROR_IMU_STILL_CALIBRATING(port, device, PROS_ERR); - - // rate is not less than 5ms, and rounded down to nearest increment of 5 - if (rate < IMU_MINIMUM_DATA_RATE) { - rate = IMU_MINIMUM_DATA_RATE; - } else { - rate -= rate % IMU_MINIMUM_DATA_RATE; - } - - vexDeviceImuDataRateSet(device->device_info, rate); - return_port(port - 1, PROS_SUCCESS); + claim_port_i(port - 1, E_DEVICE_IMU); + ERROR_IMU_STILL_CALIBRATING(port, device, PROS_ERR); + + // rate is not less than 5ms, and rounded down to nearest increment of 5 + if (rate < IMU_MINIMUM_DATA_RATE) { + rate = IMU_MINIMUM_DATA_RATE; + } else { + rate -= rate % IMU_MINIMUM_DATA_RATE; + } + + vexDeviceImuDataRateSet(device->device_info, rate); + return_port(port - 1, PROS_SUCCESS); } 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; - return_port(port - 1, rtn); + 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; + 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; - // Restricting value to raw boundaries - return_port(port - 1, fmod((rtn + IMU_HEADING_MAX), (double)IMU_HEADING_MAX)); + 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; + // Restricting value to raw boundaries + return_port(port - 1, fmod((rtn + IMU_HEADING_MAX), (double)IMU_HEADING_MAX)); } #define QUATERNION_ERR_INIT {.x = PROS_ERR_F, .y = PROS_ERR_F, .z = PROS_ERR_F, .w = PROS_ERR_F} quaternion_s_t imu_get_quaternion(uint8_t port) { - quaternion_s_t rtn = QUATERNION_ERR_INIT; - if (!claim_port_try(port - 1, E_DEVICE_IMU)) { - return rtn; - } - v5_smart_device_s_t* device = registry_get_device(port - 1); - ERROR_IMU_STILL_CALIBRATING(port, device, rtn); - euler_s_t euler; - vexDeviceImuAttitudeGet(device->device_info, (V5_DeviceImuAttitude*)&euler); - imu_data_s_t* data = (imu_data_s_t*)device->pad; - // To calculate the quaternion values, we first get the euler values, add the offsets, - // and then do the calculations. - double roll = fmod(euler.roll + data->roll_offset, 2.0 * IMU_EULER_LIMIT); - double yaw = fmod(euler.yaw + data->yaw_offset, 2.0 * IMU_EULER_LIMIT); - double pitch = fmod(euler.pitch + data->pitch_offset, 2.0 * IMU_EULER_LIMIT); - - double cy = cos(DEGTORAD * yaw * 0.5); - double sy = sin(DEGTORAD * yaw * 0.5); - 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); - - rtn.w = cr * cp * cy + sr * sp * sy; - rtn.x = sr * cp * cy - cr * sp * sy; - rtn.y = cr * sp * cy + sr * cp * sy; - rtn.z = cr * cp * sy - sr * sp * cy; - - return_port(port - 1, rtn); + quaternion_s_t rtn = QUATERNION_ERR_INIT; + if (!claim_port_try(port - 1, E_DEVICE_IMU)) { + return rtn; + } + v5_smart_device_s_t* device = registry_get_device(port - 1); + ERROR_IMU_STILL_CALIBRATING(port, device, rtn); + euler_s_t euler; + vexDeviceImuAttitudeGet(device->device_info, (V5_DeviceImuAttitude*)&euler); + imu_data_s_t* data = (imu_data_s_t*)device->pad; + // To calculate the quaternion values, we first get the euler values, add the offsets, + // and then do the calculations. + double roll = fmod(euler.roll + data->roll_offset, 2.0 * IMU_EULER_LIMIT); + double yaw = fmod(euler.yaw + data->yaw_offset, 2.0 * IMU_EULER_LIMIT); + double pitch = fmod(euler.pitch + data->pitch_offset, 2.0 * IMU_EULER_LIMIT); + + double cy = cos(DEGTORAD * yaw * 0.5); + double sy = sin(DEGTORAD * yaw * 0.5); + 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); + + rtn.w = cr * cp * cy + sr * sp * sy; + rtn.x = sr * cp * cy - cr * sp * sy; + rtn.y = cr * sp * cy + sr * cp * sy; + rtn.z = cr * cp * sy - sr * sp * cy; + + return_port(port - 1, rtn); } #define ATTITUDE_ERR_INIT {.pitch = PROS_ERR_F, .roll = PROS_ERR_F, .yaw = PROS_ERR_F} euler_s_t imu_get_euler(uint8_t port) { - euler_s_t rtn = ATTITUDE_ERR_INIT; - if (!claim_port_try(port - 1, E_DEVICE_IMU)) { - return rtn; - } - v5_smart_device_s_t* device = registry_get_device(port - 1); - imu_data_s_t* data = (imu_data_s_t*)device->pad; - ERROR_IMU_STILL_CALIBRATING(port, device, rtn); - vexDeviceImuAttitudeGet(device->device_info, (V5_DeviceImuAttitude*)&rtn); - rtn.pitch += data->pitch_offset; - rtn.yaw += data->yaw_offset; - rtn.roll += data->roll_offset; - rtn.roll = fmod(rtn.roll, 2.0 * IMU_EULER_LIMIT); - rtn.yaw = fmod(rtn.yaw, 2.0 * IMU_EULER_LIMIT); - rtn.pitch = fmod(rtn.pitch, 2.0 * IMU_EULER_LIMIT); - return_port(port - 1, rtn); + euler_s_t rtn = ATTITUDE_ERR_INIT; + if (!claim_port_try(port - 1, E_DEVICE_IMU)) { + return rtn; + } + v5_smart_device_s_t* device = registry_get_device(port - 1); + imu_data_s_t* data = (imu_data_s_t*)device->pad; + ERROR_IMU_STILL_CALIBRATING(port, device, rtn); + vexDeviceImuAttitudeGet(device->device_info, (V5_DeviceImuAttitude*)&rtn); + rtn.pitch += data->pitch_offset; + rtn.yaw += data->yaw_offset; + rtn.roll += data->roll_offset; + rtn.roll = fmod(rtn.roll, 2.0 * IMU_EULER_LIMIT); + rtn.yaw = fmod(rtn.yaw, 2.0 * IMU_EULER_LIMIT); + rtn.pitch = fmod(rtn.pitch, 2.0 * IMU_EULER_LIMIT); + return_port(port - 1, rtn); } double imu_get_pitch(uint8_t port) { - double rtn = PROS_ERR_F; - if (!claim_port_try(port - 1, E_DEVICE_IMU)) { - return rtn; - } - euler_s_t euler_values; - v5_smart_device_s_t* device = registry_get_device(port - 1); - vexDeviceImuAttitudeGet(device->device_info, (V5_DeviceImuAttitude*)&euler_values); - rtn = euler_values.pitch + ((imu_data_s_t*)registry_get_device(port - 1)->pad)->pitch_offset; - // Restricting value to raw boundaries - return_port(port - 1, fmod(rtn, 2.0 * IMU_EULER_LIMIT)); + double rtn = PROS_ERR_F; + if (!claim_port_try(port - 1, E_DEVICE_IMU)) { + return rtn; + } + euler_s_t euler_values; + v5_smart_device_s_t* device = registry_get_device(port - 1); + vexDeviceImuAttitudeGet(device->device_info, (V5_DeviceImuAttitude*)&euler_values); + rtn = euler_values.pitch + ((imu_data_s_t*)registry_get_device(port - 1)->pad)->pitch_offset; + // Restricting value to raw boundaries + return_port(port - 1, fmod(rtn, 2.0 * IMU_EULER_LIMIT)); } double imu_get_roll(uint8_t port) { - double rtn = PROS_ERR_F; - if (!claim_port_try(port - 1, E_DEVICE_IMU)) { - return rtn; - } - euler_s_t euler_values; - v5_smart_device_s_t* device = registry_get_device(port - 1); - vexDeviceImuAttitudeGet(device->device_info, (V5_DeviceImuAttitude*)&euler_values); - rtn = euler_values.roll + ((imu_data_s_t*)registry_get_device(port - 1)->pad)->roll_offset; - // Restricting value to raw boundaries - return_port(port - 1, fmod(rtn, 2.0 * IMU_EULER_LIMIT)); + double rtn = PROS_ERR_F; + if (!claim_port_try(port - 1, E_DEVICE_IMU)) { + return rtn; + } + euler_s_t euler_values; + v5_smart_device_s_t* device = registry_get_device(port - 1); + vexDeviceImuAttitudeGet(device->device_info, (V5_DeviceImuAttitude*)&euler_values); + rtn = euler_values.roll + ((imu_data_s_t*)registry_get_device(port - 1)->pad)->roll_offset; + // Restricting value to raw boundaries + return_port(port - 1, fmod(rtn, 2.0 * IMU_EULER_LIMIT)); } double imu_get_yaw(uint8_t port) { - double rtn = PROS_ERR_F; - if (!claim_port_try(port - 1, E_DEVICE_IMU)) { - return rtn; - } - euler_s_t euler_values; - v5_smart_device_s_t* device = registry_get_device(port - 1); - vexDeviceImuAttitudeGet(device->device_info, (V5_DeviceImuAttitude*)&euler_values); - rtn = euler_values.yaw + ((imu_data_s_t*)registry_get_device(port - 1)->pad)->yaw_offset; - // Restricting value to raw boundaries - return_port(port - 1, fmod(rtn, 2.0 * IMU_EULER_LIMIT)); + double rtn = PROS_ERR_F; + if (!claim_port_try(port - 1, E_DEVICE_IMU)) { + return rtn; + } + euler_s_t euler_values; + v5_smart_device_s_t* device = registry_get_device(port - 1); + vexDeviceImuAttitudeGet(device->device_info, (V5_DeviceImuAttitude*)&euler_values); + rtn = euler_values.yaw + ((imu_data_s_t*)registry_get_device(port - 1)->pad)->yaw_offset; + // Restricting value to raw boundaries + return_port(port - 1, fmod(rtn, 2.0 * IMU_EULER_LIMIT)); } #define RAW_IMU_ERR_INIT {.x = PROS_ERR_F, .y = PROS_ERR_F, .z = PROS_ERR_F}; imu_gyro_s_t imu_get_gyro_rate(uint8_t port) { - imu_gyro_s_t rtn = RAW_IMU_ERR_INIT; - if (!claim_port_try(port - 1, E_DEVICE_IMU)) { - return rtn; - } - v5_smart_device_s_t* device = registry_get_device(port - 1); - ERROR_IMU_STILL_CALIBRATING(port, device, rtn); - // NOTE: `V5_DeviceImuRaw` has the same form as a quaternion, but this call - // never fills the `w` field, so we make a dummy quaternion container and copy - // the (x,y,z) part into the return struct - quaternion_s_t dummy; - vexDeviceImuRawGyroGet(device->device_info, (V5_DeviceImuRaw*)&dummy); - rtn.x = dummy.x; - rtn.y = dummy.y; - rtn.z = dummy.z; - return_port(port - 1, rtn); + imu_gyro_s_t rtn = RAW_IMU_ERR_INIT; + if (!claim_port_try(port - 1, E_DEVICE_IMU)) { + return rtn; + } + v5_smart_device_s_t* device = registry_get_device(port - 1); + ERROR_IMU_STILL_CALIBRATING(port, device, rtn); + // NOTE: `V5_DeviceImuRaw` has the same form as a quaternion, but this call + // never fills the `w` field, so we make a dummy quaternion container and copy + // the (x,y,z) part into the return struct + quaternion_s_t dummy; + vexDeviceImuRawGyroGet(device->device_info, (V5_DeviceImuRaw*)&dummy); + rtn.x = dummy.x; + rtn.y = dummy.y; + rtn.z = dummy.z; + return_port(port - 1, rtn); } imu_accel_s_t imu_get_accel(uint8_t port) { - imu_accel_s_t rtn = RAW_IMU_ERR_INIT; - if (!claim_port_try(port - 1, E_DEVICE_IMU)) { - return rtn; - } - v5_smart_device_s_t* device = registry_get_device(port - 1); - ERROR_IMU_STILL_CALIBRATING(port, device, rtn); - // NOTE: this is the same as `imu_get_raw_gyro` - quaternion_s_t dummy; - vexDeviceImuRawAccelGet(device->device_info, (V5_DeviceImuRaw*)&dummy); - rtn.x = dummy.x; - rtn.y = dummy.y; - rtn.z = dummy.z; - return_port(port - 1, rtn); + imu_accel_s_t rtn = RAW_IMU_ERR_INIT; + if (!claim_port_try(port - 1, E_DEVICE_IMU)) { + return rtn; + } + v5_smart_device_s_t* device = registry_get_device(port - 1); + ERROR_IMU_STILL_CALIBRATING(port, device, rtn); + // NOTE: this is the same as `imu_get_raw_gyro` + quaternion_s_t dummy; + vexDeviceImuRawAccelGet(device->device_info, (V5_DeviceImuRaw*)&dummy); + rtn.x = dummy.x; + rtn.y = dummy.y; + rtn.z = dummy.z; + return_port(port - 1, rtn); } imu_status_e_t imu_get_status(uint8_t port) { - imu_status_e_t rtn = E_IMU_STATUS_ERROR; - if (!claim_port_try(port - 1, E_DEVICE_IMU)) { - return rtn; - } - v5_smart_device_s_t* device = registry_get_device(port - 1); - rtn = vexDeviceImuStatusGet(device->device_info); - return_port(port - 1, rtn); + imu_status_e_t rtn = E_IMU_STATUS_ERROR; + if (!claim_port_try(port - 1, E_DEVICE_IMU)) { + return rtn; + } + v5_smart_device_s_t* device = registry_get_device(port - 1); + rtn = vexDeviceImuStatusGet(device->device_info); + return_port(port - 1, rtn); } // 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); - euler_s_t euler_values; - vexDeviceImuAttitudeGet(device->device_info, (V5_DeviceImuAttitude*)&euler_values); - imu_data_s_t* data = (imu_data_s_t*)device->pad; - data->rotation_offset = -vexDeviceImuHeadingGet(device->device_info); - data->heading_offset = -vexDeviceImuDegreesGet(device->device_info); - data->pitch_offset = -euler_values.pitch; - data->roll_offset = -euler_values.roll; - data->yaw_offset = -euler_values.yaw; - return_port(port - 1, PROS_SUCCESS); + if (!claim_port_try(port - 1, E_DEVICE_IMU)) { + return PROS_ERR; + } + v5_smart_device_s_t* device = registry_get_device(port - 1); + euler_s_t euler_values; + vexDeviceImuAttitudeGet(device->device_info, (V5_DeviceImuAttitude*)&euler_values); + imu_data_s_t* data = (imu_data_s_t*)device->pad; + data->rotation_offset = -vexDeviceImuHeadingGet(device->device_info); + data->heading_offset = -vexDeviceImuDegreesGet(device->device_info); + data->pitch_offset = -euler_values.pitch; + data->roll_offset = -euler_values.roll; + data->yaw_offset = -euler_values.yaw; + 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}); + 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); + return imu_set_heading(port, 0); } int32_t imu_tare_rotation(uint8_t port) { - return imu_set_rotation(port, 0); + return imu_set_rotation(port, 0); } int32_t imu_tare_pitch(uint8_t port) { - return imu_set_pitch(port, 0); + return imu_set_pitch(port, 0); } int32_t imu_tare_roll(uint8_t port) { - return imu_set_roll(port, 0); + return imu_set_roll(port, 0); } int32_t imu_tare_yaw(uint8_t port) { - return imu_set_yaw(port, 0); + 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)) { - return PROS_ERR; - } - v5_smart_device_s_t* device = registry_get_device(port - 1); - ERROR_IMU_STILL_CALIBRATING(port, device, PROS_ERR); - euler_s_t euler_values; - vexDeviceImuAttitudeGet(device->device_info, (V5_DeviceImuAttitude*)&euler_values); - imu_data_s_t* data = (imu_data_s_t*)device->pad; - data->rotation_offset = target - vexDeviceImuHeadingGet(device->device_info); - return_port(port - 1, PROS_SUCCESS); + if (!claim_port_try(port - 1, E_DEVICE_IMU)) { + return PROS_ERR; + } + v5_smart_device_s_t* device = registry_get_device(port - 1); + ERROR_IMU_STILL_CALIBRATING(port, device, PROS_ERR); + euler_s_t euler_values; + vexDeviceImuAttitudeGet(device->device_info, (V5_DeviceImuAttitude*)&euler_values); + imu_data_s_t* data = (imu_data_s_t*)device->pad; + data->rotation_offset = target - vexDeviceImuHeadingGet(device->device_info); + 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)) { - return PROS_ERR; - } - v5_smart_device_s_t* device = registry_get_device(port - 1); - ERROR_IMU_STILL_CALIBRATING(port, device, PROS_ERR); - euler_s_t euler_values; - vexDeviceImuAttitudeGet(device->device_info, (V5_DeviceImuAttitude*)&euler_values); - imu_data_s_t* data = (imu_data_s_t*)device->pad; - if (target > IMU_HEADING_MAX) target = IMU_HEADING_MAX; - if (target < 0) target = 0; - data->heading_offset = target - vexDeviceImuDegreesGet(device->device_info); - return_port(port - 1, PROS_SUCCESS); + if (!claim_port_try(port - 1, E_DEVICE_IMU)) { + return PROS_ERR; + } + v5_smart_device_s_t* device = registry_get_device(port - 1); + ERROR_IMU_STILL_CALIBRATING(port, device, PROS_ERR); + euler_s_t euler_values; + vexDeviceImuAttitudeGet(device->device_info, (V5_DeviceImuAttitude*)&euler_values); + imu_data_s_t* data = (imu_data_s_t*)device->pad; + if (target > IMU_HEADING_MAX) + target = IMU_HEADING_MAX; + if (target < 0) + target = 0; + data->heading_offset = target - vexDeviceImuDegreesGet(device->device_info); + 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)) { - return PROS_ERR; - } - v5_smart_device_s_t* device = registry_get_device(port - 1); - ERROR_IMU_STILL_CALIBRATING(port, device, PROS_ERR); - euler_s_t euler_values; - vexDeviceImuAttitudeGet(device->device_info, (V5_DeviceImuAttitude*)&euler_values); - imu_data_s_t* data = (imu_data_s_t*)device->pad; - if (target > IMU_EULER_LIMIT) target = IMU_EULER_LIMIT; - if (target < -IMU_EULER_LIMIT) target = -IMU_EULER_LIMIT; - data->pitch_offset = target - euler_values.pitch; - return_port(port - 1, PROS_SUCCESS); + if (!claim_port_try(port - 1, E_DEVICE_IMU)) { + return PROS_ERR; + } + v5_smart_device_s_t* device = registry_get_device(port - 1); + ERROR_IMU_STILL_CALIBRATING(port, device, PROS_ERR); + euler_s_t euler_values; + vexDeviceImuAttitudeGet(device->device_info, (V5_DeviceImuAttitude*)&euler_values); + imu_data_s_t* data = (imu_data_s_t*)device->pad; + if (target > IMU_EULER_LIMIT) + target = IMU_EULER_LIMIT; + if (target < -IMU_EULER_LIMIT) + target = -IMU_EULER_LIMIT; + data->pitch_offset = target - euler_values.pitch; + 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)) { - return PROS_ERR; - } - v5_smart_device_s_t* device = registry_get_device(port - 1); - ERROR_IMU_STILL_CALIBRATING(port, device, PROS_ERR); - euler_s_t euler_values; - vexDeviceImuAttitudeGet(device->device_info, (V5_DeviceImuAttitude*)&euler_values); - imu_data_s_t* data = (imu_data_s_t*)device->pad; - if (target > IMU_EULER_LIMIT) target = IMU_EULER_LIMIT; - if (target < -IMU_EULER_LIMIT) target = -IMU_EULER_LIMIT; - data->roll_offset = target - euler_values.roll; - return_port(port - 1, PROS_SUCCESS); + if (!claim_port_try(port - 1, E_DEVICE_IMU)) { + return PROS_ERR; + } + v5_smart_device_s_t* device = registry_get_device(port - 1); + ERROR_IMU_STILL_CALIBRATING(port, device, PROS_ERR); + euler_s_t euler_values; + vexDeviceImuAttitudeGet(device->device_info, (V5_DeviceImuAttitude*)&euler_values); + imu_data_s_t* data = (imu_data_s_t*)device->pad; + if (target > IMU_EULER_LIMIT) + target = IMU_EULER_LIMIT; + if (target < -IMU_EULER_LIMIT) + target = -IMU_EULER_LIMIT; + data->roll_offset = target - euler_values.roll; + 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)) { - return PROS_ERR; - } - v5_smart_device_s_t* device = registry_get_device(port - 1); - ERROR_IMU_STILL_CALIBRATING(port, device, PROS_ERR); - euler_s_t euler_values; - vexDeviceImuAttitudeGet(device->device_info, (V5_DeviceImuAttitude*)&euler_values); - imu_data_s_t* data = (imu_data_s_t*)device->pad; - data->yaw_offset = target - euler_values.yaw; - if (target > IMU_EULER_LIMIT) target = IMU_EULER_LIMIT; - if (target < -IMU_EULER_LIMIT) target = -IMU_EULER_LIMIT; - return_port(port - 1, PROS_SUCCESS); + if (!claim_port_try(port - 1, E_DEVICE_IMU)) { + return PROS_ERR; + } + v5_smart_device_s_t* device = registry_get_device(port - 1); + ERROR_IMU_STILL_CALIBRATING(port, device, PROS_ERR); + euler_s_t euler_values; + vexDeviceImuAttitudeGet(device->device_info, (V5_DeviceImuAttitude*)&euler_values); + imu_data_s_t* data = (imu_data_s_t*)device->pad; + data->yaw_offset = target - euler_values.yaw; + if (target > IMU_EULER_LIMIT) + target = IMU_EULER_LIMIT; + if (target < -IMU_EULER_LIMIT) + target = -IMU_EULER_LIMIT; + return_port(port - 1, PROS_SUCCESS); } int32_t imu_set_euler(uint8_t port, euler_s_t target) { - if (!claim_port_try(port - 1, E_DEVICE_IMU)) { - return PROS_ERR; - } - v5_smart_device_s_t* device = registry_get_device(port - 1); - euler_s_t euler_values; - vexDeviceImuAttitudeGet(device->device_info, (V5_DeviceImuAttitude*)&euler_values); - imu_data_s_t* data = (imu_data_s_t*)device->pad; - if (target.pitch > IMU_EULER_LIMIT) target.pitch = IMU_EULER_LIMIT; - if (target.pitch < -IMU_EULER_LIMIT) target.pitch = -IMU_EULER_LIMIT; - if (target.yaw > IMU_EULER_LIMIT) target.yaw = IMU_EULER_LIMIT; - if (target.yaw < -IMU_EULER_LIMIT) target.yaw = -IMU_EULER_LIMIT; - if (target.roll > IMU_EULER_LIMIT) target.roll = IMU_EULER_LIMIT; - if (target.roll < -IMU_EULER_LIMIT) target.roll = -IMU_EULER_LIMIT; - data->pitch_offset = target.pitch - euler_values.pitch; - data->roll_offset = target.roll - euler_values.roll; - data->yaw_offset = target.yaw - euler_values.yaw; - return_port(port - 1, PROS_SUCCESS); + if (!claim_port_try(port - 1, E_DEVICE_IMU)) { + return PROS_ERR; + } + v5_smart_device_s_t* device = registry_get_device(port - 1); + euler_s_t euler_values; + vexDeviceImuAttitudeGet(device->device_info, (V5_DeviceImuAttitude*)&euler_values); + imu_data_s_t* data = (imu_data_s_t*)device->pad; + if (target.pitch > IMU_EULER_LIMIT) + target.pitch = IMU_EULER_LIMIT; + if (target.pitch < -IMU_EULER_LIMIT) + target.pitch = -IMU_EULER_LIMIT; + if (target.yaw > IMU_EULER_LIMIT) + target.yaw = IMU_EULER_LIMIT; + if (target.yaw < -IMU_EULER_LIMIT) + target.yaw = -IMU_EULER_LIMIT; + if (target.roll > IMU_EULER_LIMIT) + target.roll = IMU_EULER_LIMIT; + if (target.roll < -IMU_EULER_LIMIT) + target.roll = -IMU_EULER_LIMIT; + data->pitch_offset = target.pitch - euler_values.pitch; + data->roll_offset = target.roll - euler_values.roll; + 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; + 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 18efae24..53ebab47 100644 --- a/src/devices/vdml_imu.cpp +++ b/src/devices/vdml_imu.cpp @@ -10,176 +10,174 @@ * file, You can obtain one at http://mozilla.org/MPL/2.0/. */ -#include - #include "pros/imu.hpp" -#include "vdml/vdml.h" +#include "vdml/vdml.hpp" + +#include namespace pros { inline namespace v5 { std::int32_t Imu::reset(bool blocking /*= false*/) const { - return blocking ? pros::c::imu_reset_blocking(_port) : pros::c::imu_reset(_port); + return blocking ? pros::c::imu_reset_blocking(_port) : pros::c::imu_reset(_port); } std::int32_t Imu::set_data_rate(std::uint32_t rate) const { - return pros::c::imu_set_data_rate(_port, rate); + 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 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; + 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); + return pros::c::imu_get_rotation(_port); } double Imu::get_heading() const { - return pros::c::imu_get_heading(_port); + return pros::c::imu_get_heading(_port); } pros::quaternion_s_t Imu::get_quaternion() const { - return pros::c::imu_get_quaternion(_port); + return pros::c::imu_get_quaternion(_port); } pros::euler_s_t Imu::get_euler() const { - return pros::c::imu_get_euler(_port); + return pros::c::imu_get_euler(_port); } double Imu::get_pitch() const { - return get_euler().pitch; + return get_euler().pitch; } double Imu::get_roll() const { - return get_euler().roll; + return get_euler().roll; } double Imu::get_yaw() const { - return get_euler().yaw; + return get_euler().yaw; } pros::imu_gyro_s_t Imu::get_gyro_rate() const { - return pros::c::imu_get_gyro_rate(_port); + return pros::c::imu_get_gyro_rate(_port); } pros::imu_accel_s_t Imu::get_accel() const { - return pros::c::imu_get_accel(_port); + return pros::c::imu_get_accel(_port); } pros::ImuStatus Imu::get_status() const { - return static_cast(pros::c::imu_get_status(_port)); + return static_cast(pros::c::imu_get_status(_port)); } bool Imu::is_calibrating() const { - 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; + 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 { - return pros::c::imu_tare_heading(_port); + return pros::c::imu_tare_heading(_port); } std::int32_t Imu::tare_rotation() const { - return pros::c::imu_tare_rotation(_port); + return pros::c::imu_tare_rotation(_port); } std::int32_t Imu::tare_pitch() const { - return pros::c::imu_tare_pitch(_port); + return pros::c::imu_tare_pitch(_port); } std::int32_t Imu::tare_yaw() const { - return pros::c::imu_tare_yaw(_port); + return pros::c::imu_tare_yaw(_port); } std::int32_t Imu::tare_roll() const { - return pros::c::imu_tare_roll(_port); + return pros::c::imu_tare_roll(_port); } std::int32_t Imu::tare_euler() const { - return pros::c::imu_tare_euler(_port); + return pros::c::imu_tare_euler(_port); } std::int32_t Imu::set_heading(double target) const { - return pros::c::imu_set_heading(_port, target); + return pros::c::imu_set_heading(_port, target); } std::int32_t Imu::set_rotation(double target) const { - return pros::c::imu_set_rotation(_port, target); + return pros::c::imu_set_rotation(_port, target); } std::int32_t Imu::set_pitch(double target) const { - return pros::c::imu_set_pitch(_port, target); + return pros::c::imu_set_pitch(_port, target); } std::int32_t Imu::set_yaw(double target) const { - return pros::c::imu_set_yaw(_port, target); + return pros::c::imu_set_yaw(_port, target); } std::int32_t Imu::set_roll(double target) const { - return pros::c::imu_set_roll(_port, target); + return pros::c::imu_set_roll(_port, target); } std::int32_t Imu::set_euler(pros::euler_s_t target) const { - return pros::c::imu_set_euler(_port, target); + return pros::c::imu_set_euler(_port, target); } std::int32_t Imu::tare() const { - return pros::c::imu_tare(_port); + return pros::c::imu_tare(_port); } imu_orientation_e_t Imu::get_physical_orientation() const { - return pros::c::imu_get_physical_orientation(_port); + 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); + 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(); - os << "Imu ["; - os << "port: " << imu._port; - os << ", rotation: " << imu.get_rotation(); - os << ", heading: " << imu.get_heading(); - os << ", pitch: " << imu.get_pitch(); - os << ", roll: " << imu.get_roll(); - os << ", yaw: " << imu.get_yaw(); - os << ", gyro rate: " - << "{" << gyro.x << "," << gyro.y << "," << gyro.z << "}"; - os << ", get accel: " - << "{" << accel.x << "," << accel.y << "," << accel.z << "}"; - os << ", calibrating: " << imu.is_calibrating(); - os << "]"; - return os; + pros::imu_gyro_s_t gyro = imu.get_gyro_rate(); + pros::imu_accel_s_t accel = imu.get_accel(); + os << "Imu ["; + os << "port: " << imu._port; + os << ", rotation: " << imu.get_rotation(); + os << ", heading: " << imu.get_heading(); + os << ", pitch: " << imu.get_pitch(); + os << ", roll: " << imu.get_roll(); + os << ", yaw: " << imu.get_yaw(); + os << ", gyro rate: " + << "{" << gyro.x << "," << gyro.y << "," << gyro.z << "}"; + os << ", get accel: " + << "{" << accel.x << "," << accel.y << "," << accel.z << "}"; + os << ", calibrating: " << imu.is_calibrating(); + os << "]"; + return os; } namespace literals { const pros::Imu operator"" _imu(const unsigned long long int i) { - return pros::Imu(i); + return pros::Imu(i); } -} // namespace literals -} // namespace v5 -} // namespace pros +} // namespace literals +} // namespace v5 +} // namespace pros diff --git a/src/devices/vdml_link.c b/src/devices/vdml_link.c index aa73b68d..d5083510 100644 --- a/src/devices/vdml_link.c +++ b/src/devices/vdml_link.c @@ -13,40 +13,44 @@ * file, You can obtain one at http://mozilla.org/MPL/2.0/. */ - #include "pros/link.h" - #include "kapi.h" - #include "pros/apix.h" - #include "vdml/vdml.h" - #include +#include "kapi.h" +#include "pros/apix.h" +#include "pros/link.h" +#include "vdml/vdml.hpp" -#define PROTOCOL_SIZE 4 // Protocol Size +#include + +#define PROTOCOL_SIZE 4 // Protocol Size static const uint8_t START_BYTE = 0x33; -// internal function for clearing the rx buffer +// internal function for clearing the rx buffer static uint32_t _clear_rx_buf(v5_smart_device_s_t* device) { uint8_t buf[LINK_BUFFER_SIZE]; - return vexDeviceGenericRadioReceive(device->device_info, - (uint8_t*)buf, - vexDeviceGenericRadioReceiveAvail(device->device_info)); + return vexDeviceGenericRadioReceive( + device->device_info, + (uint8_t*)buf, + vexDeviceGenericRadioReceiveAvail(device->device_info) + ); } -//custom claim_port style wrapper for link_init due to type mismatching otherwise, limit of one radio per brain -static uint32_t _link_init(uint8_t port, const char* link_id, link_type_e_t type, bool ov) -{ +// custom claim_port style wrapper for link_init due to type mismatching otherwise, limit of one +// radio per brain +static uint32_t _link_init(uint8_t port, const char* link_id, link_type_e_t type, bool ov) { if (!port_mutex_take(port)) { - errno = EACCES; - return PROS_ERR; + errno = EACCES; + return PROS_ERR; } v5_device_e_t plugged_type = registry_get_plugged_type(port); if (plugged_type == E_DEVICE_RADIO) { if (!VALIDATE_PORT_NO(port)) { - errno = ENXIO; - return PROS_ERR; - } + errno = ENXIO; + return PROS_ERR; + } v5_smart_device_s_t* device = registry_get_device(port); - vexDeviceGenericRadioConnection(device->device_info, (char* )link_id, type, ov); - // Hack: Force v5 to recognize the plugged type as a serial device the next time we touch the device + vexDeviceGenericRadioConnection(device->device_info, (char*)link_id, type, ov); + // Hack: Force v5 to recognize the plugged type as a serial device the next time we touch + // the device registry_unbind_port(port); return_port(port, PROS_SUCCESS); } @@ -64,13 +68,13 @@ uint32_t link_init_override(uint8_t port, const char* link_id, link_type_e_t typ bool link_connected(uint8_t port) { claim_port(port - 1, E_DEVICE_SERIAL, false); - bool rtv = vexDeviceGenericRadioLinkStatus(device->device_info); + bool rtv = vexDeviceGenericRadioLinkStatus(device->device_info); return_port(port - 1, rtv); } uint32_t link_raw_receivable_size(uint8_t port) { claim_port_i(port - 1, E_DEVICE_SERIAL); - uint32_t rtv = vexDeviceGenericRadioReceiveAvail(device->device_info); + uint32_t rtv = vexDeviceGenericRadioReceiveAvail(device->device_info); return_port(port - 1, rtv); } @@ -81,16 +85,16 @@ uint32_t link_raw_transmittable_size(uint8_t port) { } uint32_t link_transmit_raw(uint8_t port, void* data, uint16_t data_size) { - if(data == NULL) { + if (data == NULL) { errno = EINVAL; return PROS_ERR; } claim_port_i(port - 1, E_DEVICE_SERIAL); - if(!vexDeviceGenericRadioLinkStatus(device->device_info)) { + if (!vexDeviceGenericRadioLinkStatus(device->device_info)) { errno = ENXIO; return_port(port - 1, PROS_ERR); } - if(data_size > vexDeviceGenericRadioWriteFree(device->device_info)) { + if (data_size > vexDeviceGenericRadioWriteFree(device->device_info)) { errno = EBUSY; return_port(port - 1, PROS_ERR); } @@ -99,16 +103,16 @@ uint32_t link_transmit_raw(uint8_t port, void* data, uint16_t data_size) { } uint32_t link_receive_raw(uint8_t port, void* dest, uint16_t data_size) { - if(dest == NULL) { + if (dest == NULL) { errno = EINVAL; return PROS_ERR; } - claim_port_i(port - 1, E_DEVICE_SERIAL); - if(!vexDeviceGenericRadioLinkStatus(device->device_info)) { + claim_port_i(port - 1, E_DEVICE_SERIAL); + if (!vexDeviceGenericRadioLinkStatus(device->device_info)) { errno = ENXIO; return_port(port - 1, PROS_ERR); } - if(data_size > vexDeviceGenericRadioReceiveAvail(device->device_info)) { + if (data_size > vexDeviceGenericRadioReceiveAvail(device->device_info)) { errno = EBUSY; return_port(port - 1, PROS_ERR); } @@ -117,16 +121,16 @@ uint32_t link_receive_raw(uint8_t port, void* dest, uint16_t data_size) { } uint32_t link_transmit(uint8_t port, void* data, uint16_t data_size) { - if(data == NULL) { + if (data == NULL) { errno = EINVAL; return PROS_ERR; } claim_port_i(port - 1, E_DEVICE_SERIAL); - if(!vexDeviceGenericRadioLinkStatus(device->device_info)) { + if (!vexDeviceGenericRadioLinkStatus(device->device_info)) { errno = ENXIO; return_port(port - 1, PROS_ERR); } - if(data_size + PROTOCOL_SIZE > vexDeviceGenericRadioWriteFree(device->device_info)) { + if (data_size + PROTOCOL_SIZE > vexDeviceGenericRadioWriteFree(device->device_info)) { errno = EBUSY; return_port(port - 1, PROS_ERR); } @@ -137,8 +141,8 @@ uint32_t link_transmit(uint8_t port, void* data, uint16_t data_size) { size_tx_buf[0] = (data_size) & 0xff; checksum ^= size_tx_buf[1]; checksum ^= size_tx_buf[0]; - - for(int i = 0; i < data_size; i++) { + + for (int i = 0; i < data_size; i++) { checksum ^= ((uint8_t*)data)[i]; } uint32_t rtv = 0; @@ -151,16 +155,16 @@ uint32_t link_transmit(uint8_t port, void* data, uint16_t data_size) { } uint32_t link_receive(uint8_t port, void* dest, uint16_t data_size) { - if(dest == NULL) { + if (dest == NULL) { errno = EINVAL; return PROS_ERR; } claim_port_i(port - 1, E_DEVICE_SERIAL); - if(!vexDeviceGenericRadioLinkStatus(device->device_info)) { + if (!vexDeviceGenericRadioLinkStatus(device->device_info)) { errno = ENXIO; return_port(port - 1, PROS_ERR); } - if(data_size + PROTOCOL_SIZE > vexDeviceGenericRadioReceiveAvail(device->device_info)) { + if (data_size + PROTOCOL_SIZE > vexDeviceGenericRadioReceiveAvail(device->device_info)) { errno = EBUSY; return_port(port - 1, PROS_ERR); } @@ -168,23 +172,32 @@ uint32_t link_receive(uint8_t port, void* dest, uint16_t data_size) { uint8_t received_size; // process protocol received_size = vexDeviceGenericRadioReceive(device->device_info, &header_byte, 1); // 0x33 - if(START_BYTE != header_byte || received_size != 1) { - kprintf("[VEXLINK] Invalid Header Byte Received Port %d, header byte: %x", port, header_byte); + if (START_BYTE != header_byte || received_size != 1) { + kprintf( + "[VEXLINK] Invalid Header Byte Received Port %d, header byte: %x", + port, + header_byte + ); errno = EBADMSG; return_port(port - 1, PROS_ERR); } // datasize uint16_t received_data_size; - received_size = vexDeviceGenericRadioReceive(device->device_info, (uint8_t*)&received_data_size, 2); - if(received_size != 2 || received_data_size != data_size) { + received_size = + vexDeviceGenericRadioReceive(device->device_info, (uint8_t*)&received_data_size, 2); + if (received_size != 2 || received_data_size != data_size) { _clear_rx_buf(device); - kprintf("[VEXLINK] Invalid Data Size (Size: %d ) Received Port %d, flushing RX buffer!\n", received_data_size, port); + kprintf( + "[VEXLINK] Invalid Data Size (Size: %d ) Received Port %d, flushing RX buffer!\n", + received_data_size, + port + ); errno = EBADMSG; return_port(port - 1, PROS_ERR); } // receive data uint32_t rtv = vexDeviceGenericRadioReceive(device->device_info, (uint8_t*)dest, data_size); - if(rtv != data_size || received_data_size != data_size) { + if (rtv != data_size || received_data_size != data_size) { kprintf("[VEXLINK] Invalid Data Received Port %d, flushing RX buffer!\n", port); errno = EBADMSG; _clear_rx_buf(device); @@ -196,10 +209,10 @@ uint32_t link_receive(uint8_t port, void* dest, uint16_t data_size) { uint8_t calculated_checksum = START_BYTE; calculated_checksum ^= (data_size >> 8) & 0xff; calculated_checksum ^= (data_size) & 0xff; - for(int i = 0; i < data_size; i++) { + for (int i = 0; i < data_size; i++) { calculated_checksum ^= ((uint8_t*)dest)[i]; } - if(calculated_checksum != received_checksum || received_size != 1) { + if (calculated_checksum != received_checksum || received_size != 1) { kprintf("[VEXLINK] Checksum Mismatch Port %d!, Checksum: %x\n", port, received_checksum); errno = EBADMSG; return_port(port - 1, PROS_ERR); @@ -212,4 +225,3 @@ uint32_t link_clear_receive_buf(uint8_t port) { uint32_t rtv = _clear_rx_buf(device); return_port(port - 1, rtv); } - diff --git a/src/devices/vdml_link.cpp b/src/devices/vdml_link.cpp index 044482f3..917a2bdf 100644 --- a/src/devices/vdml_link.cpp +++ b/src/devices/vdml_link.cpp @@ -5,7 +5,7 @@ * * This file should not be modified by users, since it gets replaced whenever * a kernel upgrade occurs. - * + * * \copyright Copyright (c) 2017-2024, Purdue University ACM SIGBots. * * This Source Code Form is subject to the terms of the Mozilla Public @@ -14,42 +14,44 @@ */ #include "pros/link.hpp" -#include "vdml/vdml.h" +#include "vdml/vdml.hpp" namespace pros { -Link::Link(const std::uint8_t port, const std::string link_id, link_type_e_t type, bool ov) : Device(port, DeviceType::radio) { - (ov) ? pros::c::link_init_override(_port, link_id.c_str(), type) : pros::c::link_init(_port, link_id.c_str(), type); +Link::Link(const std::uint8_t port, const std::string link_id, link_type_e_t type, bool ov) + : Device(port, DeviceType::radio) { + (ov) ? pros::c::link_init_override(_port, link_id.c_str(), type) + : pros::c::link_init(_port, link_id.c_str(), type); } bool Link::connected() { - return pros::c::link_connected(_port); + return pros::c::link_connected(_port); } std::uint32_t Link::raw_receivable_size() { - return pros::c::link_raw_receivable_size(_port); + return pros::c::link_raw_receivable_size(_port); } std::uint32_t Link::raw_transmittable_size() { - return pros::c::link_raw_transmittable_size(_port); + return pros::c::link_raw_transmittable_size(_port); } std::uint32_t Link::transmit_raw(void* data, std::uint16_t data_size) { - return pros::c::link_transmit_raw(_port, data, data_size); + return pros::c::link_transmit_raw(_port, data, data_size); } std::uint32_t Link::receive_raw(void* dest, std::uint16_t data_size) { - return pros::c::link_receive_raw(_port, dest, data_size); + return pros::c::link_receive_raw(_port, dest, data_size); } std::uint32_t Link::transmit(void* data, std::uint16_t data_size) { - return pros::c::link_transmit(_port, data, data_size); + return pros::c::link_transmit(_port, data, data_size); } std::uint32_t Link::receive(void* dest, std::uint16_t data_size) { - return pros::c::link_receive(_port, dest, data_size); + return pros::c::link_receive(_port, dest, data_size); } std::uint32_t Link::clear_receive_buf() { - return pros::c::link_clear_receive_buf(_port); + return pros::c::link_clear_receive_buf(_port); } -} // namespace pros +} // namespace pros diff --git a/src/devices/vdml_motors.c b/src/devices/vdml_motors.c index 8eb4889c..39e2d49b 100644 --- a/src/devices/vdml_motors.c +++ b/src/devices/vdml_motors.c @@ -10,17 +10,17 @@ * file, You can obtain one at http://mozilla.org/MPL/2.0/. */ -#include -#include -#include - #include "kapi.h" +#include "pros/apix.h" #include "pros/colors.h" #include "pros/motors.h" -#include "pros/apix.h" #include "v5_api.h" -#include "vdml/registry.h" -#include "vdml/vdml.h" +#include "vdml/registry.hpp" +#include "vdml/vdml.hpp" + +#include +#include +#include #define MOTOR_MOVE_RANGE 127 #define MOTOR_VOLTAGE_RANGE 12000 @@ -28,292 +28,304 @@ // Movement functions int32_t motor_move(int8_t port, int32_t voltage) { - if (voltage > 127) { - voltage = 127; - } else if (voltage < -127) { - voltage = -127; - } + if (voltage > 127) { + voltage = 127; + } else if (voltage < -127) { + voltage = -127; + } - if (port < 0) { - voltage = -voltage; - } + if (port < 0) { + voltage = -voltage; + } - port = abs(port); - // Remap the input voltage range to the motor voltage - // scale to [-127, 127] -> [-12000, 12000] - int32_t command = (((voltage + MOTOR_MOVE_RANGE) * (MOTOR_VOLTAGE_RANGE)) / (MOTOR_MOVE_RANGE)); - command -= MOTOR_VOLTAGE_RANGE; - return motor_move_voltage(port, command); + port = abs(port); + // Remap the input voltage range to the motor voltage + // scale to [-127, 127] -> [-12000, 12000] + int32_t command = (((voltage + MOTOR_MOVE_RANGE) * (MOTOR_VOLTAGE_RANGE)) / (MOTOR_MOVE_RANGE)); + command -= MOTOR_VOLTAGE_RANGE; + return motor_move_voltage(port, command); } int32_t motor_brake(int8_t port) { - return motor_move_velocity(port, 0); + return motor_move_velocity(port, 0); } int32_t motor_move_absolute(int8_t port, double position, int32_t velocity) { - uint8_t abs_port = abs(port); - claim_port_i(abs_port - 1, E_DEVICE_MOTOR); - if (port < 0) position = -position; - vexDeviceMotorAbsoluteTargetSet(device->device_info, position, velocity); - return_port(abs_port - 1, PROS_SUCCESS); + uint8_t abs_port = abs(port); + claim_port_i(abs_port - 1, E_DEVICE_MOTOR); + if (port < 0) + position = -position; + vexDeviceMotorAbsoluteTargetSet(device->device_info, position, velocity); + return_port(abs_port - 1, PROS_SUCCESS); } int32_t motor_move_relative(int8_t port, double position, int32_t velocity) { - uint8_t abs_port = abs(port); - claim_port_i(abs_port - 1, E_DEVICE_MOTOR); - if (port < 0) position = -position; - vexDeviceMotorRelativeTargetSet(device->device_info, position, velocity); - return_port(abs_port - 1, PROS_SUCCESS); + uint8_t abs_port = abs(port); + claim_port_i(abs_port - 1, E_DEVICE_MOTOR); + if (port < 0) + position = -position; + vexDeviceMotorRelativeTargetSet(device->device_info, position, velocity); + return_port(abs_port - 1, PROS_SUCCESS); } int32_t motor_move_velocity(int8_t port, int32_t velocity) { - uint8_t abs_port = abs(port); - claim_port_i(abs_port - 1, E_DEVICE_MOTOR); - if (port < 0) velocity = -velocity; - vexDeviceMotorVelocitySet(device->device_info, velocity); - return_port(abs_port - 1, PROS_SUCCESS); + uint8_t abs_port = abs(port); + claim_port_i(abs_port - 1, E_DEVICE_MOTOR); + if (port < 0) + velocity = -velocity; + vexDeviceMotorVelocitySet(device->device_info, velocity); + return_port(abs_port - 1, PROS_SUCCESS); } int32_t motor_move_voltage(int8_t port, int32_t voltage) { - uint8_t abs_port = abs(port); - claim_port_i(abs_port - 1, E_DEVICE_MOTOR); - if (port < 0) voltage = -voltage; - vexDeviceMotorVoltageSet(device->device_info, voltage); - return_port(abs_port - 1, PROS_SUCCESS); + uint8_t abs_port = abs(port); + claim_port_i(abs_port - 1, E_DEVICE_MOTOR); + if (port < 0) + voltage = -voltage; + vexDeviceMotorVoltageSet(device->device_info, voltage); + return_port(abs_port - 1, PROS_SUCCESS); } int32_t motor_modify_profiled_velocity(int8_t port, int32_t velocity) { - uint8_t abs_port = abs(port); - claim_port_i(abs_port - 1, E_DEVICE_MOTOR); - if (port < 0) velocity = -velocity; - vexDeviceMotorVelocityUpdate(device->device_info, velocity); - return_port(abs_port - 1, PROS_SUCCESS); + uint8_t abs_port = abs(port); + claim_port_i(abs_port - 1, E_DEVICE_MOTOR); + if (port < 0) + velocity = -velocity; + vexDeviceMotorVelocityUpdate(device->device_info, velocity); + return_port(abs_port - 1, PROS_SUCCESS); } double motor_get_target_position(int8_t port) { - uint8_t abs_port = abs(port); - claim_port_f(abs_port - 1, E_DEVICE_MOTOR); - double rtn = vexDeviceMotorTargetGet(device->device_info); - if (port < 0) rtn = -rtn; - return_port(abs_port - 1, rtn); + uint8_t abs_port = abs(port); + claim_port_f(abs_port - 1, E_DEVICE_MOTOR); + double rtn = vexDeviceMotorTargetGet(device->device_info); + if (port < 0) + rtn = -rtn; + return_port(abs_port - 1, rtn); } int32_t motor_get_target_velocity(int8_t port) { - uint8_t abs_port = abs(port); - claim_port_i(abs_port - 1, E_DEVICE_MOTOR); - int32_t rtn = vexDeviceMotorVelocityGet(device->device_info); - if (port < 0) rtn = -rtn; - return_port(abs_port - 1, rtn); + uint8_t abs_port = abs(port); + claim_port_i(abs_port - 1, E_DEVICE_MOTOR); + int32_t rtn = vexDeviceMotorVelocityGet(device->device_info); + if (port < 0) + rtn = -rtn; + return_port(abs_port - 1, rtn); } // Telemetry functions double motor_get_actual_velocity(int8_t port) { - uint8_t abs_port = abs(port); - claim_port_f(abs_port - 1, E_DEVICE_MOTOR); - double rtn = vexDeviceMotorActualVelocityGet(device->device_info); - if (port < 0) rtn = -rtn; - return_port(abs_port - 1, rtn); + uint8_t abs_port = abs(port); + claim_port_f(abs_port - 1, E_DEVICE_MOTOR); + double rtn = vexDeviceMotorActualVelocityGet(device->device_info); + if (port < 0) + rtn = -rtn; + return_port(abs_port - 1, rtn); } int32_t motor_get_current_draw(int8_t port) { - port = abs(port); - claim_port_i(port - 1, E_DEVICE_MOTOR); - int32_t rtn = vexDeviceMotorCurrentGet(device->device_info); - return_port(port - 1, rtn); + port = abs(port); + claim_port_i(port - 1, E_DEVICE_MOTOR); + int32_t rtn = vexDeviceMotorCurrentGet(device->device_info); + return_port(port - 1, rtn); } int32_t motor_get_direction(int8_t port) { - uint8_t abs_port = abs(port); - claim_port_i(abs_port - 1, E_DEVICE_MOTOR); - int32_t rtn = vexDeviceMotorDirectionGet(device->device_info); - if (port < 0) rtn = -rtn; - return_port(abs_port - 1, rtn); + uint8_t abs_port = abs(port); + claim_port_i(abs_port - 1, E_DEVICE_MOTOR); + int32_t rtn = vexDeviceMotorDirectionGet(device->device_info); + if (port < 0) + rtn = -rtn; + return_port(abs_port - 1, rtn); } double motor_get_efficiency(int8_t port) { - port = abs(port); - claim_port_f(port - 1, E_DEVICE_MOTOR); - double rtn = vexDeviceMotorEfficiencyGet(device->device_info); - return_port(port - 1, rtn); + port = abs(port); + claim_port_f(port - 1, E_DEVICE_MOTOR); + double rtn = vexDeviceMotorEfficiencyGet(device->device_info); + return_port(port - 1, rtn); } int32_t motor_is_over_current(int8_t port) { - port = abs(port); - claim_port_i(port - 1, E_DEVICE_MOTOR); - int rtn = vexDeviceMotorCurrentLimitFlagGet(device->device_info); - return_port(port - 1, rtn); + port = abs(port); + claim_port_i(port - 1, E_DEVICE_MOTOR); + int rtn = vexDeviceMotorCurrentLimitFlagGet(device->device_info); + return_port(port - 1, rtn); } int32_t motor_is_over_temp(int8_t port) { - port = abs(port); - claim_port_i(port - 1, E_DEVICE_MOTOR); - int rtn = vexDeviceMotorOverTempFlagGet(device->device_info); - return_port(port - 1, rtn); + port = abs(port); + claim_port_i(port - 1, E_DEVICE_MOTOR); + int rtn = vexDeviceMotorOverTempFlagGet(device->device_info); + return_port(port - 1, rtn); } uint32_t motor_get_faults(int8_t port) { - port = abs(port); - claim_port_i(port - 1, E_DEVICE_MOTOR); - uint32_t rtn = vexDeviceMotorFaultsGet(device->device_info); - return_port(port - 1, rtn); + port = abs(port); + claim_port_i(port - 1, E_DEVICE_MOTOR); + uint32_t rtn = vexDeviceMotorFaultsGet(device->device_info); + return_port(port - 1, rtn); } uint32_t motor_get_flags(int8_t port) { - port = abs(port); - claim_port_i(port - 1, E_DEVICE_MOTOR); - uint32_t rtn = vexDeviceMotorFlagsGet(device->device_info); - return_port(port - 1, rtn); + port = abs(port); + claim_port_i(port - 1, E_DEVICE_MOTOR); + uint32_t rtn = vexDeviceMotorFlagsGet(device->device_info); + return_port(port - 1, rtn); } int32_t motor_get_raw_position(int8_t port, uint32_t* const timestamp) { - uint8_t abs_port = abs(port); - claim_port_i(abs_port - 1, E_DEVICE_MOTOR); - int32_t rtn = vexDeviceMotorPositionRawGet(device->device_info, timestamp); - if (port < 0) rtn = -rtn; - return_port(abs_port - 1, rtn); + uint8_t abs_port = abs(port); + claim_port_i(abs_port - 1, E_DEVICE_MOTOR); + int32_t rtn = vexDeviceMotorPositionRawGet(device->device_info, timestamp); + if (port < 0) + rtn = -rtn; + return_port(abs_port - 1, rtn); } double motor_get_position(int8_t port) { - uint8_t abs_port = abs(port); - claim_port_f(abs_port - 1, E_DEVICE_MOTOR); - double rtn = vexDeviceMotorPositionGet(device->device_info); - if (port < 0) rtn = -rtn; - return_port(abs_port - 1, rtn); + uint8_t abs_port = abs(port); + claim_port_f(abs_port - 1, E_DEVICE_MOTOR); + double rtn = vexDeviceMotorPositionGet(device->device_info); + if (port < 0) + rtn = -rtn; + return_port(abs_port - 1, rtn); } double motor_get_power(int8_t port) { - port = abs(port); - claim_port_f(port - 1, E_DEVICE_MOTOR); - double rtn = vexDeviceMotorPowerGet(device->device_info); - return_port(port - 1, rtn); + port = abs(port); + claim_port_f(port - 1, E_DEVICE_MOTOR); + double rtn = vexDeviceMotorPowerGet(device->device_info); + return_port(port - 1, rtn); } double motor_get_temperature(int8_t port) { - port = abs(port); - claim_port_f(port - 1, E_DEVICE_MOTOR); - double rtn = vexDeviceMotorTemperatureGet(device->device_info); - return_port(port - 1, rtn); + port = abs(port); + claim_port_f(port - 1, E_DEVICE_MOTOR); + double rtn = vexDeviceMotorTemperatureGet(device->device_info); + return_port(port - 1, rtn); } double motor_get_torque(int8_t port) { - port = abs(port); - claim_port_f(port - 1, E_DEVICE_MOTOR); - double rtn = vexDeviceMotorTorqueGet(device->device_info); - return_port(port - 1, rtn); + port = abs(port); + claim_port_f(port - 1, E_DEVICE_MOTOR); + double rtn = vexDeviceMotorTorqueGet(device->device_info); + return_port(port - 1, rtn); } int32_t motor_get_voltage(int8_t port) { - uint8_t abs_port = abs(port); - claim_port_i(abs_port - 1, E_DEVICE_MOTOR); - int32_t rtn = vexDeviceMotorVoltageGet(device->device_info); - if (port < 0) rtn = -rtn; - return_port(abs_port - 1, rtn); + uint8_t abs_port = abs(port); + claim_port_i(abs_port - 1, E_DEVICE_MOTOR); + int32_t rtn = vexDeviceMotorVoltageGet(device->device_info); + if (port < 0) + rtn = -rtn; + return_port(abs_port - 1, rtn); } // Config functions int32_t motor_set_zero_position(int8_t port, const double position) { - port = abs(port); - claim_port_i(port - 1, E_DEVICE_MOTOR); - vexDeviceMotorPositionSet(device->device_info, position); - return_port(port - 1, PROS_SUCCESS); + port = abs(port); + claim_port_i(port - 1, E_DEVICE_MOTOR); + vexDeviceMotorPositionSet(device->device_info, position); + return_port(port - 1, PROS_SUCCESS); } int32_t motor_tare_position(int8_t port) { - port = abs(port); - claim_port_i(port - 1, E_DEVICE_MOTOR); - vexDeviceMotorPositionReset(device->device_info); - return_port(port - 1, PROS_SUCCESS); + port = abs(port); + claim_port_i(port - 1, E_DEVICE_MOTOR); + vexDeviceMotorPositionReset(device->device_info); + return_port(port - 1, PROS_SUCCESS); } int32_t motor_set_brake_mode(int8_t port, const motor_brake_mode_e_t mode) { - port = abs(port); - claim_port_i(port - 1, E_DEVICE_MOTOR); - vexDeviceMotorBrakeModeSet(device->device_info, (V5MotorBrakeMode)mode); - return_port(port - 1, PROS_SUCCESS); + port = abs(port); + claim_port_i(port - 1, E_DEVICE_MOTOR); + vexDeviceMotorBrakeModeSet(device->device_info, (V5MotorBrakeMode)mode); + return_port(port - 1, PROS_SUCCESS); } int32_t motor_set_current_limit(int8_t port, const int32_t limit) { - port = abs(port); - claim_port_i(port - 1, E_DEVICE_MOTOR); - vexDeviceMotorCurrentLimitSet(device->device_info, limit); - return_port(port - 1, PROS_SUCCESS); + port = abs(port); + claim_port_i(port - 1, E_DEVICE_MOTOR); + vexDeviceMotorCurrentLimitSet(device->device_info, limit); + return_port(port - 1, PROS_SUCCESS); } int32_t motor_set_encoder_units(int8_t port, const motor_encoder_units_e_t units) { - port = abs(port); - claim_port_i(port - 1, E_DEVICE_MOTOR); - vexDeviceMotorEncoderUnitsSet(device->device_info, (V5MotorEncoderUnits)units); - return_port(port - 1, PROS_SUCCESS); + port = abs(port); + claim_port_i(port - 1, E_DEVICE_MOTOR); + vexDeviceMotorEncoderUnitsSet(device->device_info, (V5MotorEncoderUnits)units); + return_port(port - 1, PROS_SUCCESS); } int32_t motor_set_gearing(int8_t port, const motor_gearset_e_t gearset) { - port = abs(port); - claim_port_i(port - 1, E_DEVICE_MOTOR); - vexDeviceMotorGearingSet(device->device_info, (V5MotorGearset)gearset); - return_port(port - 1, PROS_SUCCESS); + port = abs(port); + claim_port_i(port - 1, E_DEVICE_MOTOR); + vexDeviceMotorGearingSet(device->device_info, (V5MotorGearset)gearset); + return_port(port - 1, PROS_SUCCESS); } int32_t motor_set_reversed(int8_t port, const bool reverse) { - port = abs(port); - claim_port_i(port - 1, E_DEVICE_MOTOR); - vexDeviceMotorReverseFlagSet(device->device_info, reverse); - return_port(port - 1, PROS_SUCCESS); + port = abs(port); + claim_port_i(port - 1, E_DEVICE_MOTOR); + vexDeviceMotorReverseFlagSet(device->device_info, reverse); + return_port(port - 1, PROS_SUCCESS); } int32_t motor_set_voltage_limit(int8_t port, const int32_t limit) { - port = abs(port); - claim_port_i(port - 1, E_DEVICE_MOTOR); - vexDeviceMotorVoltageLimitSet(device->device_info, limit); - return_port(port - 1, PROS_SUCCESS); + port = abs(port); + claim_port_i(port - 1, E_DEVICE_MOTOR); + vexDeviceMotorVoltageLimitSet(device->device_info, limit); + return_port(port - 1, PROS_SUCCESS); } motor_brake_mode_e_t motor_get_brake_mode(int8_t port) { - port = abs(port); - claim_port_i(port - 1, E_DEVICE_MOTOR); - V5MotorBrakeMode rtn = vexDeviceMotorBrakeModeGet(device->device_info); - return_port(port - 1, (motor_brake_mode_e_t)rtn); + port = abs(port); + claim_port_i(port - 1, E_DEVICE_MOTOR); + V5MotorBrakeMode rtn = vexDeviceMotorBrakeModeGet(device->device_info); + return_port(port - 1, (motor_brake_mode_e_t)rtn); } int32_t motor_get_current_limit(int8_t port) { - port = abs(port); - claim_port_i(port - 1, E_DEVICE_MOTOR); - int32_t rtn = vexDeviceMotorCurrentLimitGet(device->device_info); - return_port(port - 1, rtn); + port = abs(port); + claim_port_i(port - 1, E_DEVICE_MOTOR); + int32_t rtn = vexDeviceMotorCurrentLimitGet(device->device_info); + return_port(port - 1, rtn); } motor_encoder_units_e_t motor_get_encoder_units(int8_t port) { - port = abs(port); - claim_port_i(port - 1, E_DEVICE_MOTOR); - V5MotorEncoderUnits rtn = vexDeviceMotorEncoderUnitsGet(device->device_info); - return_port(port - 1, (motor_encoder_units_e_t)rtn); + port = abs(port); + claim_port_i(port - 1, E_DEVICE_MOTOR); + V5MotorEncoderUnits rtn = vexDeviceMotorEncoderUnitsGet(device->device_info); + return_port(port - 1, (motor_encoder_units_e_t)rtn); } motor_gearset_e_t motor_get_gearing(int8_t port) { - port = abs(port); - claim_port_i(port - 1, E_DEVICE_MOTOR); - V5MotorGearset rtn = vexDeviceMotorGearingGet(device->device_info); - return_port(port - 1, (motor_gearset_e_t)rtn); + port = abs(port); + claim_port_i(port - 1, E_DEVICE_MOTOR); + V5MotorGearset rtn = vexDeviceMotorGearingGet(device->device_info); + return_port(port - 1, (motor_gearset_e_t)rtn); } int32_t motor_is_reversed(int8_t port) { - port = abs(port); - claim_port_i(port - 1, E_DEVICE_MOTOR); - int rtn = vexDeviceMotorReverseFlagGet(device->device_info); - return_port(port - 1, rtn); + port = abs(port); + claim_port_i(port - 1, E_DEVICE_MOTOR); + int rtn = vexDeviceMotorReverseFlagGet(device->device_info); + return_port(port - 1, rtn); } 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(port - 1, rtn); + port = abs(port); + claim_port_i(port - 1, E_DEVICE_MOTOR); + int32_t rtn = vexDeviceMotorVoltageLimitGet(device->device_info); + return_port(port - 1, rtn); } motor_type_e_t motor_get_type(int8_t port) { - port = abs(port); - claim_port_i(port - 1, E_DEVICE_MOTOR); - int32_t rtn = vexDeviceMotorTypeGet(device->device_info); - return_port(port - 1, rtn); + port = abs(port); + claim_port_i(port - 1, E_DEVICE_MOTOR); + int32_t rtn = vexDeviceMotorTypeGet(device->device_info); + return_port(port - 1, rtn); } diff --git a/src/devices/vdml_motors.cpp b/src/devices/vdml_motors.cpp index 8f3ab31f..dfd79ad4 100644 --- a/src/devices/vdml_motors.cpp +++ b/src/devices/vdml_motors.cpp @@ -12,565 +12,590 @@ #include "kapi.h" #include "pros/motors.hpp" -#include "vdml/vdml.h" +#include "vdml/vdml.hpp" + #include namespace pros { inline namespace v5 { using namespace pros::c; - -Motor::Motor(const std::int8_t port, const pros::v5::MotorGears gearset, const pros::v5::MotorUnits encoder_units) - : Device(std::abs(port), DeviceType::motor), _port(port) { - if (gearset != pros::v5::MotorGears::invalid) { - set_gearing(gearset); - } - if (encoder_units != pros::v5::MotorEncoderUnits::invalid) { - set_encoder_units(encoder_units); - } +Motor::Motor( + const std::int8_t port, + const pros::v5::MotorGears gearset, + const pros::v5::MotorUnits encoder_units +) + : Device(std::abs(port), DeviceType::motor), + _port(port) { + 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 { - return motor_move(_port, voltage); - + return motor_move(_port, voltage); } std::int32_t Motor::move_absolute(const double position, const std::int32_t velocity) const { - return motor_move_absolute(_port, position, velocity); + return motor_move_absolute(_port, position, velocity); } std::int32_t Motor::move_relative(const double position, const std::int32_t velocity) const { - return motor_move_relative(_port, position, velocity); + return motor_move_relative(_port, position, velocity); } std::int32_t Motor::move_velocity(const std::int32_t velocity) const { - return motor_move_velocity(_port, velocity); + return motor_move_velocity(_port, velocity); } std::int32_t Motor::move_voltage(const std::int32_t voltage) const { - return motor_move_voltage(_port, voltage); + return motor_move_voltage(_port, voltage); } std::int32_t Motor::brake(void) const { - return motor_brake(_port); + return motor_brake(_port); } std::int32_t Motor::modify_profiled_velocity(const std::int32_t velocity) const { - return motor_modify_profiled_velocity(_port, velocity); + return motor_modify_profiled_velocity(_port, velocity); } double Motor::get_actual_velocity(const std::uint8_t index) const { - if (index != 0) { - errno = EOVERFLOW; - return PROS_ERR_F; - } - return motor_get_actual_velocity(_port); + if (index != 0) { + errno = EOVERFLOW; + return PROS_ERR_F; + } + return motor_get_actual_velocity(_port); } + std::vector Motor::get_actual_velocity_all(void) const { - std::vector return_vector; - return_vector.push_back(motor_get_actual_velocity(_port)); - return return_vector; + std::vector return_vector; + return_vector.push_back(motor_get_actual_velocity(_port)); + return return_vector; } pros::v5::MotorBrake Motor::get_brake_mode(const std::uint8_t index) const { - if (index != 0) { - errno = EOVERFLOW; - return pros::v5::MotorBrake::invalid; - } - return static_cast(motor_get_brake_mode(_port)); + if (index != 0) { + errno = EOVERFLOW; + return pros::v5::MotorBrake::invalid; + } + return static_cast(motor_get_brake_mode(_port)); } std::vector Motor::get_brake_mode_all() const { - std::vector return_vector; - return_vector.push_back(static_cast(motor_get_brake_mode(_port))); - return return_vector; + std::vector return_vector; + return_vector.push_back(static_cast(motor_get_brake_mode(_port))); + return return_vector; } std::int32_t Motor::get_current_draw(const std::uint8_t index) const { - if (index != 0) { - errno = EOVERFLOW; - return PROS_ERR; - } - return motor_get_current_draw(_port); + if (index != 0) { + errno = EOVERFLOW; + return PROS_ERR; + } + return motor_get_current_draw(_port); } + std::vector Motor::get_current_draw_all(void) const { - std::vector return_vector; - return_vector.push_back(motor_get_current_draw(_port)); - return return_vector; + std::vector return_vector; + return_vector.push_back(motor_get_current_draw(_port)); + return return_vector; } + std::int32_t Motor::get_current_limit(const std::uint8_t index) const { - if (index != 0) { - errno = EOVERFLOW; - return PROS_ERR; - } - return motor_get_current_limit(_port); + if (index != 0) { + errno = EOVERFLOW; + return PROS_ERR; + } + return motor_get_current_limit(_port); } + std::vector Motor::get_current_limit_all(void) const { - std::vector return_vector; - return_vector.push_back(motor_get_current_limit(_port)); - return return_vector; + std::vector return_vector; + return_vector.push_back(motor_get_current_limit(_port)); + return return_vector; } std::int32_t Motor::is_over_current(const std::uint8_t index) const { - if (index != 0) { - errno = EOVERFLOW; - return PROS_ERR; - } - return motor_is_over_current(_port); + if (index != 0) { + errno = EOVERFLOW; + return PROS_ERR; + } + return motor_is_over_current(_port); } std::vector Motor::is_over_current_all(void) const { - std::vector return_vector; - return_vector.push_back(motor_is_over_current(_port)); + std::vector return_vector; + return_vector.push_back(motor_is_over_current(_port)); - return return_vector; + return return_vector; } std::int32_t Motor::get_direction(const std::uint8_t index) const { - if (index != 0) { - errno = EOVERFLOW; - return PROS_ERR; - } - int ret = motor_get_direction(_port); - ret = _port >= 0 ? ret : ret * -1; - return ret; + if (index != 0) { + errno = EOVERFLOW; + return PROS_ERR; + } + int ret = motor_get_direction(_port); + ret = _port >= 0 ? ret : ret * -1; + return ret; } + std::vector Motor::get_direction_all(void) const { - std::vector return_vector; - int ret = motor_get_direction(_port); - ret = _port >= 0 ? ret : ret * -1; - return_vector.push_back(ret); - return return_vector; + std::vector return_vector; + int ret = motor_get_direction(_port); + ret = _port >= 0 ? ret : ret * -1; + return_vector.push_back(ret); + return return_vector; } double Motor::get_efficiency(const std::uint8_t index) const { - if (index != 0) { - errno = EOVERFLOW; - return PROS_ERR_F; - } - return motor_get_efficiency(_port); + if (index != 0) { + errno = EOVERFLOW; + return PROS_ERR_F; + } + return motor_get_efficiency(_port); } + std::vector Motor::get_efficiency_all(void) const { - std::vector return_vector; - return_vector.push_back(motor_get_efficiency(_port)); - return return_vector; + std::vector return_vector; + return_vector.push_back(motor_get_efficiency(_port)); + return return_vector; } pros::v5::MotorUnits Motor::get_encoder_units(const std::uint8_t index) const { - if (index != 0) { - errno = EOVERFLOW; - return pros::v5::MotorUnits::invalid; - } - return static_cast(motor_get_encoder_units(_port)); + if (index != 0) { + errno = EOVERFLOW; + return pros::v5::MotorUnits::invalid; + } + return static_cast(motor_get_encoder_units(_port)); } std::vector Motor::get_encoder_units_all(void) const { - std::vector return_vector; - return_vector.push_back(static_cast(motor_get_encoder_units(_port))); - return return_vector; + std::vector return_vector; + return_vector.push_back(static_cast(motor_get_encoder_units(_port))); + return return_vector; } std::uint32_t Motor::get_faults(const std::uint8_t index) const { - if (index != 0) { - errno = EOVERFLOW; - return PROS_ERR; - } - return motor_get_faults(_port); + if (index != 0) { + errno = EOVERFLOW; + return PROS_ERR; + } + return motor_get_faults(_port); } std::vector Motor::get_faults_all(void) const { - std::vector return_vector; - return_vector.push_back(motor_get_faults(_port)); - return return_vector; + std::vector return_vector; + return_vector.push_back(motor_get_faults(_port)); + return return_vector; } std::uint32_t Motor::get_flags(const std::uint8_t index) const { - if (index != 0) { - errno = EOVERFLOW; - return PROS_ERR; - } - return motor_get_flags(_port); + if (index != 0) { + errno = EOVERFLOW; + return PROS_ERR; + } + return motor_get_flags(_port); } std::vector Motor::get_flags_all(void) const { - std::vector return_vector; - return_vector.push_back(motor_get_flags(_port)); - return return_vector; + std::vector return_vector; + return_vector.push_back(motor_get_flags(_port)); + return return_vector; } pros::v5::MotorGears Motor::get_gearing(const std::uint8_t index) const { - if (index != 0) { - errno = EOVERFLOW; - return pros::v5::MotorGears::invalid; - } - return static_cast(motor_get_gearing(_port)); + if (index != 0) { + errno = EOVERFLOW; + return pros::v5::MotorGears::invalid; + } + return static_cast(motor_get_gearing(_port)); } + std::vector Motor::get_gearing_all(void) const { - std::vector return_vector; - return_vector.push_back(static_cast(motor_get_gearing(_port))); - return return_vector; + std::vector return_vector; + return_vector.push_back(static_cast(motor_get_gearing(_port))); + return return_vector; } std::int32_t Motor::get_raw_position(std::uint32_t* const timestamp, std::uint8_t index) const { - if (index != 0) { - errno = EOVERFLOW; - return PROS_ERR; - } - return motor_get_raw_position(_port, timestamp); + if (index != 0) { + errno = EOVERFLOW; + return PROS_ERR; + } + return motor_get_raw_position(_port, timestamp); } std::vector Motor::get_raw_position_all(std::uint32_t* const timestamp) const { - std::vector return_vector; - return_vector.push_back(motor_get_raw_position(_port, timestamp)); - return return_vector; + std::vector return_vector; + return_vector.push_back(motor_get_raw_position(_port, timestamp)); + return return_vector; } std::int32_t Motor::is_over_temp(const std::uint8_t index) const { - if (index != 0) { - errno = EOVERFLOW; - return PROS_ERR; - } - return motor_is_over_temp(_port); + if (index != 0) { + errno = EOVERFLOW; + return PROS_ERR; + } + return motor_is_over_temp(_port); } std::vector Motor::is_over_temp_all(void) const { - std::vector return_vector; - return_vector.push_back(motor_is_over_temp(_port)); - return return_vector; + std::vector return_vector; + return_vector.push_back(motor_is_over_temp(_port)); + return return_vector; } double Motor::get_position(const std::uint8_t index) const { - if (index != 0) { - errno = EOVERFLOW; - return PROS_ERR_F; - } - return motor_get_position(_port); + if (index != 0) { + errno = EOVERFLOW; + return PROS_ERR_F; + } + return motor_get_position(_port); } + std::vector Motor::get_position_all(void) const { - std::vector return_vector; - return_vector.push_back(motor_get_position(_port)); - return return_vector; + std::vector return_vector; + return_vector.push_back(motor_get_position(_port)); + return return_vector; } double Motor::get_power(const std::uint8_t index) const { - if (index != 0) { - errno = EOVERFLOW; - return PROS_ERR; - } - return motor_get_power(_port); + if (index != 0) { + errno = EOVERFLOW; + return PROS_ERR; + } + return motor_get_power(_port); } std::vector Motor::get_power_all(void) const { - std::vector return_vector; - return_vector.push_back(motor_get_power(_port)); - return return_vector; + std::vector return_vector; + return_vector.push_back(motor_get_power(_port)); + return return_vector; } std::int32_t Motor::is_reversed(const std::uint8_t index) const { - if (index != 0) { - errno = EOVERFLOW; - return PROS_ERR; - } - return _port < 0; + if (index != 0) { + errno = EOVERFLOW; + return PROS_ERR; + } + return _port < 0; } + std::vector Motor::is_reversed_all(void) const { - std::vector return_vector; - return_vector.push_back(_port < 0); - return return_vector; + std::vector return_vector; + return_vector.push_back(_port < 0); + return return_vector; } pros::v5::MotorType Motor::get_type(const std::uint8_t index) const { - if (index != 0) { - errno = EOVERFLOW; - return pros::v5::MotorType::invalid; - } - return static_cast(motor_get_type(_port)); + if (index != 0) { + errno = EOVERFLOW; + return pros::v5::MotorType::invalid; + } + return static_cast(motor_get_type(_port)); } + std::vector Motor::get_type_all(void) const { - std::vector return_vector; - return_vector.push_back(static_cast(motor_get_type(_port))); - return return_vector; + std::vector return_vector; + return_vector.push_back(static_cast(motor_get_type(_port))); + return return_vector; } double Motor::get_temperature(const std::uint8_t index) const { - if (index != 0) { - errno = EOVERFLOW; - return PROS_ERR_F; - } - return motor_get_temperature(_port); + if (index != 0) { + errno = EOVERFLOW; + return PROS_ERR_F; + } + return motor_get_temperature(_port); } std::vector Motor::get_temperature_all(void) const { - std::vector return_vector; - return_vector.push_back(motor_get_temperature(_port)); - return return_vector; + std::vector return_vector; + return_vector.push_back(motor_get_temperature(_port)); + return return_vector; } double Motor::get_target_position(const std::uint8_t index) const { - if (index != 0) { - errno = EOVERFLOW; - return PROS_ERR; - } - return motor_get_target_position(_port); + if (index != 0) { + errno = EOVERFLOW; + return PROS_ERR; + } + return motor_get_target_position(_port); } std::vector Motor::get_target_position_all(void) const { - std::vector return_vector; - return_vector.push_back(motor_get_target_position(_port)); - return return_vector; + std::vector return_vector; + return_vector.push_back(motor_get_target_position(_port)); + return return_vector; } double Motor::get_torque(const std::uint8_t index) const { - if (index != 0) { - errno = EOVERFLOW; - return PROS_ERR; - } - return motor_get_torque(_port); + if (index != 0) { + errno = EOVERFLOW; + return PROS_ERR; + } + return motor_get_torque(_port); } + std::vector Motor::get_torque_all(void) const { - std::vector return_vector; - return_vector.push_back(motor_get_torque(_port)); - return return_vector; + std::vector return_vector; + return_vector.push_back(motor_get_torque(_port)); + return return_vector; } + std::int32_t Motor::get_target_velocity(const std::uint8_t index) const { - if (index != 0) { - errno = EOVERFLOW; - return PROS_ERR; - } - return motor_get_target_velocity(_port); + if (index != 0) { + errno = EOVERFLOW; + return PROS_ERR; + } + return motor_get_target_velocity(_port); } + std::vector Motor::get_target_velocity_all(void) const { - std::vector return_vector; - return_vector.push_back(motor_get_target_velocity(_port)); - return return_vector; + std::vector return_vector; + return_vector.push_back(motor_get_target_velocity(_port)); + return return_vector; } std::int32_t Motor::get_voltage(const std::uint8_t index) const { - if (index != 0) { - errno = EOVERFLOW; - return PROS_ERR; - } - return motor_get_voltage(_port); + if (index != 0) { + errno = EOVERFLOW; + return PROS_ERR; + } + return motor_get_voltage(_port); } + std::vector Motor::get_voltage_all(void) const { - std::vector return_vector; - return_vector.push_back(motor_get_voltage(_port)); - return return_vector; + std::vector return_vector; + return_vector.push_back(motor_get_voltage(_port)); + return return_vector; } std::int32_t Motor::get_voltage_limit(const std::uint8_t index) const { - if (index != 0) { - errno = EOVERFLOW; - return PROS_ERR; - } - return motor_get_voltage_limit(_port); + if (index != 0) { + errno = EOVERFLOW; + return PROS_ERR; + } + 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 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::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; - return PROS_ERR_BYTE; - } - return _port; + if (index != 0) { + errno = EOVERFLOW; + return PROS_ERR_BYTE; + } + return _port; } std::vector Motor::get_port_all(void) const { - std::vector return_vector; - return_vector.push_back(_port); - return return_vector; + std::vector return_vector; + return_vector.push_back(_port); + return return_vector; } std::int32_t Motor::tare_position(const std::uint8_t index) const { - if (index != 0) { - errno = EOVERFLOW; - return PROS_ERR; - } - return motor_tare_position(_port); + if (index != 0) { + errno = EOVERFLOW; + return PROS_ERR; + } + return motor_tare_position(_port); } -std::int32_t Motor::set_brake_mode(const pros::motor_brake_mode_e_t mode, const std::uint8_t index) const { - if (index != 0) { - errno = EOVERFLOW; - return PROS_ERR; - } - return motor_set_brake_mode(_port, mode); +std::int32_t +Motor::set_brake_mode(const pros::motor_brake_mode_e_t mode, const std::uint8_t index) const { + if (index != 0) { + errno = EOVERFLOW; + return PROS_ERR; + } + return motor_set_brake_mode(_port, mode); } -std::int32_t Motor::set_brake_mode(const pros::v5::MotorBrake mode, const std::uint8_t index) const { - if (index != 0) { - errno = EOVERFLOW; - return PROS_ERR; - } - return motor_set_brake_mode(_port, static_cast(mode)); +std::int32_t +Motor::set_brake_mode(const pros::v5::MotorBrake mode, const std::uint8_t index) const { + if (index != 0) { + errno = EOVERFLOW; + return PROS_ERR; + } + return motor_set_brake_mode(_port, static_cast(mode)); } std::int32_t Motor::set_current_limit(const std::int32_t limit, const std::uint8_t index) const { - if (index != 0) { - errno = EOVERFLOW; - return PROS_ERR; - } - return motor_set_current_limit(_port, limit); + if (index != 0) { + errno = EOVERFLOW; + return PROS_ERR; + } + return motor_set_current_limit(_port, limit); } -std::int32_t Motor::set_encoder_units(const pros::motor_encoder_units_e_t units, const std::uint8_t index) const { - if (index != 0) { - errno = EOVERFLOW; - return PROS_ERR; - } - return motor_set_encoder_units(_port, units); +std::int32_t Motor::set_encoder_units( + const pros::motor_encoder_units_e_t units, + const std::uint8_t index +) const { + if (index != 0) { + errno = EOVERFLOW; + return PROS_ERR; + } + return motor_set_encoder_units(_port, units); } -std::int32_t Motor::set_encoder_units(const pros::v5::MotorUnits units, const std::uint8_t index) const { - if (index != 0) { - errno = EOVERFLOW; - return PROS_ERR; - } - return motor_set_encoder_units(_port, static_cast(units)); +std::int32_t +Motor::set_encoder_units(const pros::v5::MotorUnits units, const std::uint8_t index) const { + if (index != 0) { + errno = EOVERFLOW; + return PROS_ERR; + } + return motor_set_encoder_units(_port, static_cast(units)); } std::int32_t Motor::set_gearing(const motor_gearset_e_t gearset, const std::uint8_t index) const { - if (index != 0) { - errno = EOVERFLOW; - return PROS_ERR; - } - return motor_set_gearing(_port, gearset); + if (index != 0) { + errno = EOVERFLOW; + return PROS_ERR; + } + return motor_set_gearing(_port, gearset); } std::int32_t Motor::set_gearing(const pros::v5::MotorGear gearset, const std::uint8_t index) const { - if (index != 0) { - errno = EOVERFLOW; - return PROS_ERR; - } - return motor_set_gearing(_port, static_cast(gearset)); + if (index != 0) { + errno = EOVERFLOW; + return PROS_ERR; + } + return motor_set_gearing(_port, static_cast(gearset)); } std::int32_t Motor::set_zero_position(const double position, const std::uint8_t index) const { - if (index != 0) { - errno = EOVERFLOW; - return PROS_ERR; - } - return motor_set_zero_position(_port, position); + if (index != 0) { + errno = EOVERFLOW; + return PROS_ERR; + } + return motor_set_zero_position(_port, position); } std::int32_t Motor::set_reversed(const bool reverse, const std::uint8_t index) { - if (index != 0) { - errno = EOVERFLOW; - return PROS_ERR; - } - std::int8_t abs_port = std::abs(_port); - if (reverse) { - _port = -abs_port; - } else { - _port = abs_port; - - } - return PROS_SUCCESS; + if (index != 0) { + errno = EOVERFLOW; + return PROS_ERR; + } + std::int8_t abs_port = std::abs(_port); + if (reverse) { + _port = -abs_port; + } else { + _port = abs_port; + } + return PROS_SUCCESS; } std::int32_t Motor::set_voltage_limit(const std::int32_t limit, const std::uint8_t index) const { - if (index != 0) { - errno = EOVERFLOW; - return PROS_ERR; - } - return motor_set_voltage_limit(_port, limit); + if (index != 0) { + errno = EOVERFLOW; + return PROS_ERR; + } + return motor_set_voltage_limit(_port, limit); } std::int32_t Motor::tare_position_all(void) const { - return motor_tare_position(_port); + return motor_tare_position(_port); } std::int32_t Motor::set_brake_mode_all(const pros::motor_brake_mode_e_t mode) const { - return motor_set_brake_mode(_port, mode); + return motor_set_brake_mode(_port, mode); } std::int32_t Motor::set_brake_mode_all(const pros::v5::MotorBrake mode) const { - return motor_set_brake_mode(_port, static_cast(mode)); + return motor_set_brake_mode(_port, static_cast(mode)); } std::int32_t Motor::set_current_limit_all(const std::int32_t limit) const { - return motor_set_current_limit(_port, limit); + return motor_set_current_limit(_port, limit); } std::int32_t Motor::set_encoder_units_all(const pros::motor_encoder_units_e_t units) const { - return motor_set_encoder_units(_port, units); + return motor_set_encoder_units(_port, units); } std::int32_t Motor::set_encoder_units_all(const pros::v5::MotorUnits units) const { - return motor_set_encoder_units(_port, static_cast(units)); + return motor_set_encoder_units(_port, static_cast(units)); } std::int32_t Motor::set_gearing_all(const motor_gearset_e_t gearset) const { - return motor_set_gearing(_port, gearset); + return motor_set_gearing(_port, gearset); } std::int32_t Motor::set_gearing_all(const pros::v5::MotorGear gearset) const { - return motor_set_gearing(_port, static_cast(gearset)); + return motor_set_gearing(_port, static_cast(gearset)); } std::int32_t Motor::set_zero_position_all(const double position) const { - return motor_set_zero_position(_port, position); + return motor_set_zero_position(_port, position); } std::int32_t Motor::set_reversed_all(const bool reverse) { - std::int8_t abs_port = std::abs(_port); - if (reverse) { - _port = -abs_port; - } else { - _port = abs_port; - } - return PROS_SUCCESS; + std::int8_t abs_port = std::abs(_port); + if (reverse) { + _port = -abs_port; + } else { + _port = abs_port; + } + return PROS_SUCCESS; } std::int32_t Motor::set_voltage_limit_all(const std::int32_t limit) const { - return motor_set_voltage_limit(_port, limit); + return motor_set_voltage_limit(_port, limit); } std::int8_t Motor::size() const { - return 1; + return 1; } std::ostream& operator<<(std::ostream& os, pros::Motor& motor) { - os << "Motor ["; - os << "port: " << motor.get_port(); - os << ", brake mode: " << (int)motor.get_brake_mode(); - os << ", current draw: " << motor.get_current_draw(); - os << ", current limit: " << motor.get_current_limit(); - os << ", direction: " << motor.get_direction(); - os << ", efficiency: " << motor.get_efficiency(); - os << ", encoder units: " << (int)motor.get_encoder_units(); - os << ", gearing: " << (int)motor.get_gearing(); - os << ", over temp: " << motor.is_over_temp(); - os << ", position: " << motor.get_position(); - os << ", reversed: " << motor.is_reversed(); - os << ", temperature: " << motor.get_temperature(); - os << ", torque: " << motor.get_torque(); - os << ", voltage: " << motor.get_voltage(); - os << "]"; - return os; -} - -} // namespace v5 + os << "Motor ["; + os << "port: " << motor.get_port(); + os << ", brake mode: " << (int)motor.get_brake_mode(); + os << ", current draw: " << motor.get_current_draw(); + os << ", current limit: " << motor.get_current_limit(); + os << ", direction: " << motor.get_direction(); + os << ", efficiency: " << motor.get_efficiency(); + os << ", encoder units: " << (int)motor.get_encoder_units(); + os << ", gearing: " << (int)motor.get_gearing(); + os << ", over temp: " << motor.is_over_temp(); + os << ", position: " << motor.get_position(); + os << ", reversed: " << motor.is_reversed(); + os << ", temperature: " << motor.get_temperature(); + os << ", torque: " << motor.get_torque(); + os << ", voltage: " << motor.get_voltage(); + os << "]"; + return os; +} + +} // namespace v5 + namespace literals { const pros::Motor operator"" _mtr(const unsigned long long int m) { - return pros::Motor(m); + return pros::Motor(m); } + const pros::Motor operator"" _rmtr(const unsigned long long int m) { - return pros::Motor(-m); + return pros::Motor(-m); } -} // namespace literals -} // namespace pros +} // namespace literals +} // namespace pros diff --git a/src/devices/vdml_optical.c b/src/devices/vdml_optical.c index e525a4c5..515f2fce 100644 --- a/src/devices/vdml_optical.c +++ b/src/devices/vdml_optical.c @@ -10,149 +10,152 @@ * file, You can obtain one at http://mozilla.org/MPL/2.0/. */ -#include - #include "pros/optical.h" #include "v5_api.h" -#include "vdml/registry.h" -#include "vdml/vdml.h" +#include "vdml/registry.hpp" +#include "vdml/vdml.hpp" + +#include -// Source for these figures: +// Source for these figures: // https://www.vexforum.com/t/v5-optical-sensor-refresh-rate/109632/9 -#define MIN_INTEGRATION_TIME 3 // ms +#define MIN_INTEGRATION_TIME 3 // ms #define MAX_INTEGRATION_TIME 712 // ms double optical_get_hue(uint8_t port) { - claim_port_i(port - 1, E_DEVICE_OPTICAL); - double rtn = vexDeviceOpticalHueGet(device->device_info); - return_port(port - 1, rtn); + claim_port_i(port - 1, E_DEVICE_OPTICAL); + double rtn = vexDeviceOpticalHueGet(device->device_info); + return_port(port - 1, rtn); } double optical_get_saturation(uint8_t port) { - claim_port_i(port - 1, E_DEVICE_OPTICAL); - double rtn = vexDeviceOpticalSatGet(device->device_info); - return_port(port - 1, rtn); + claim_port_i(port - 1, E_DEVICE_OPTICAL); + double rtn = vexDeviceOpticalSatGet(device->device_info); + return_port(port - 1, rtn); } double optical_get_brightness(uint8_t port) { - claim_port_i(port - 1, E_DEVICE_OPTICAL); - double rtn = vexDeviceOpticalBrightnessGet(device->device_info); - return_port(port - 1, rtn); + claim_port_i(port - 1, E_DEVICE_OPTICAL); + double rtn = vexDeviceOpticalBrightnessGet(device->device_info); + return_port(port - 1, rtn); } int32_t optical_get_proximity(uint8_t port) { - claim_port_i(port - 1, E_DEVICE_OPTICAL); - double rtn = vexDeviceOpticalProximityGet(device->device_info); - return_port(port - 1, rtn); + claim_port_i(port - 1, E_DEVICE_OPTICAL); + double rtn = vexDeviceOpticalProximityGet(device->device_info); + return_port(port - 1, rtn); } int32_t optical_set_led_pwm(uint8_t port, uint8_t value) { - claim_port_i(port - 1, E_DEVICE_OPTICAL); - vexDeviceOpticalLedPwmSet(device->device_info, value); - return_port(port - 1, PROS_SUCCESS); + claim_port_i(port - 1, E_DEVICE_OPTICAL); + vexDeviceOpticalLedPwmSet(device->device_info, value); + return_port(port - 1, PROS_SUCCESS); } int32_t optical_get_led_pwm(uint8_t port) { - claim_port_i(port - 1, E_DEVICE_OPTICAL); - int32_t rtn = vexDeviceOpticalLedPwmGet(device->device_info); - return_port(port - 1, rtn); + claim_port_i(port - 1, E_DEVICE_OPTICAL); + int32_t rtn = vexDeviceOpticalLedPwmGet(device->device_info); + return_port(port - 1, rtn); } -#define RGB_ERR_INIT \ - { .red = PROS_ERR_F, .green = PROS_ERR_F, .blue = PROS_ERR_F, .brightness = PROS_ERR_F } +#define RGB_ERR_INIT \ + {.red = PROS_ERR_F, .green = PROS_ERR_F, .blue = PROS_ERR_F, .brightness = PROS_ERR_F} optical_rgb_s_t optical_get_rgb(uint8_t port) { - optical_rgb_s_t rtn = RGB_ERR_INIT; - v5_smart_device_s_t* device; - if (!claim_port_try(port - 1, E_DEVICE_OPTICAL)) { - return rtn; - } - device = registry_get_device(port - 1); - V5_DeviceOpticalRgb rgb; - vexDeviceOpticalRgbGet(device->device_info, &rgb); - rtn.red = rgb.red; - rtn.green = rgb.green; - rtn.blue = rgb.blue; - rtn.brightness = rgb.brightness; - return_port(port - 1, rtn); + optical_rgb_s_t rtn = RGB_ERR_INIT; + v5_smart_device_s_t* device; + if (!claim_port_try(port - 1, E_DEVICE_OPTICAL)) { + return rtn; + } + device = registry_get_device(port - 1); + V5_DeviceOpticalRgb rgb; + vexDeviceOpticalRgbGet(device->device_info, &rgb); + rtn.red = rgb.red; + rtn.green = rgb.green; + rtn.blue = rgb.blue; + rtn.brightness = rgb.brightness; + return_port(port - 1, rtn); } -#define RAW_ERR_INIT \ - { .clear = PROS_ERR, .red = PROS_ERR, .green = PROS_ERR, .blue = PROS_ERR } +#define RAW_ERR_INIT {.clear = PROS_ERR, .red = PROS_ERR, .green = PROS_ERR, .blue = PROS_ERR} optical_raw_s_t optical_get_raw(uint8_t port) { - optical_raw_s_t rtn = RAW_ERR_INIT; - v5_smart_device_s_t* device; - if (!claim_port_try(port - 1, E_DEVICE_OPTICAL)) { - return rtn; - } - device = registry_get_device(port - 1); - V5_DeviceOpticalRaw rgb; - vexDeviceOpticalRawGet(device->device_info, &rgb); - rtn.clear = rgb.clear; - rtn.red = rgb.red; - rtn.green = rgb.green; - rtn.blue = rgb.blue; - return_port(port - 1, rtn); + optical_raw_s_t rtn = RAW_ERR_INIT; + v5_smart_device_s_t* device; + if (!claim_port_try(port - 1, E_DEVICE_OPTICAL)) { + return rtn; + } + device = registry_get_device(port - 1); + V5_DeviceOpticalRaw rgb; + vexDeviceOpticalRawGet(device->device_info, &rgb); + rtn.clear = rgb.clear; + rtn.red = rgb.red; + rtn.green = rgb.green; + rtn.blue = rgb.blue; + return_port(port - 1, rtn); } optical_direction_e_t optical_get_gesture(uint8_t port) { - claim_port(port - 1, E_DEVICE_OPTICAL, OPT_GESTURE_ERR); - optical_direction_e_t rtn = vexDeviceOpticalGestureGet(device->device_info, NULL); - return_port(port - 1, rtn); + claim_port(port - 1, E_DEVICE_OPTICAL, OPT_GESTURE_ERR); + optical_direction_e_t rtn = vexDeviceOpticalGestureGet(device->device_info, NULL); + return_port(port - 1, rtn); } -#define GESTURE_ERR_INIT \ - { \ - .udata = OPT_GESTURE_ERR, .ddata = OPT_GESTURE_ERR, .ldata = OPT_GESTURE_ERR, .rdata = OPT_GESTURE_ERR, \ - .type = OPT_GESTURE_ERR, .pad = OPT_GESTURE_ERR, .count = OPT_COUNT_ERR, .time = OPT_TIME_ERR \ - } +#define GESTURE_ERR_INIT \ + {.udata = OPT_GESTURE_ERR, \ + .ddata = OPT_GESTURE_ERR, \ + .ldata = OPT_GESTURE_ERR, \ + .rdata = OPT_GESTURE_ERR, \ + .type = OPT_GESTURE_ERR, \ + .pad = OPT_GESTURE_ERR, \ + .count = OPT_COUNT_ERR, \ + .time = OPT_TIME_ERR} optical_gesture_s_t optical_get_gesture_raw(uint8_t port) { - optical_gesture_s_t rtn = GESTURE_ERR_INIT; - v5_smart_device_s_t* device; - if (!claim_port_try(port - 1, E_DEVICE_OPTICAL)) { - return rtn; - } - device = registry_get_device(port - 1); - V5_DeviceOpticalGesture gesture; - vexDeviceOpticalGestureGet(device->device_info, &gesture); - rtn.udata = gesture.udata; - rtn.ddata = gesture.ddata; - rtn.ldata = gesture.ldata; - rtn.rdata = gesture.rdata; - rtn.type = gesture.type; - rtn.pad = gesture.pad; - rtn.count = gesture.count; - rtn.time = gesture.time; - return_port(port - 1, rtn); + optical_gesture_s_t rtn = GESTURE_ERR_INIT; + v5_smart_device_s_t* device; + if (!claim_port_try(port - 1, E_DEVICE_OPTICAL)) { + return rtn; + } + device = registry_get_device(port - 1); + V5_DeviceOpticalGesture gesture; + vexDeviceOpticalGestureGet(device->device_info, &gesture); + rtn.udata = gesture.udata; + rtn.ddata = gesture.ddata; + rtn.ldata = gesture.ldata; + rtn.rdata = gesture.rdata; + rtn.type = gesture.type; + rtn.pad = gesture.pad; + rtn.count = gesture.count; + rtn.time = gesture.time; + return_port(port - 1, rtn); } int32_t optical_enable_gesture(uint8_t port) { - claim_port_i(port - 1, E_DEVICE_OPTICAL); - vexDeviceOpticalGestureEnable(device->device_info); - return_port(port - 1, PROS_SUCCESS); + claim_port_i(port - 1, E_DEVICE_OPTICAL); + vexDeviceOpticalGestureEnable(device->device_info); + return_port(port - 1, PROS_SUCCESS); } int32_t optical_disable_gesture(uint8_t port) { - claim_port_i(port - 1, E_DEVICE_OPTICAL); - vexDeviceOpticalGestureDisable(device->device_info); - return_port(port - 1, PROS_SUCCESS); + claim_port_i(port - 1, E_DEVICE_OPTICAL); + vexDeviceOpticalGestureDisable(device->device_info); + return_port(port - 1, PROS_SUCCESS); } double optical_get_integration_time(uint8_t port) { - claim_port_f(port - 1, E_DEVICE_OPTICAL); - double rtv = vexDeviceOpticalIntegrationTimeGet(device->device_info); - return_port(port - 1, rtv); + claim_port_f(port - 1, E_DEVICE_OPTICAL); + double rtv = vexDeviceOpticalIntegrationTimeGet(device->device_info); + return_port(port - 1, rtv); } int32_t optical_set_integration_time(uint8_t port, double time) { - claim_port_i(port - 1, E_DEVICE_OPTICAL); - // going to set the time to minimum of 3 ms, 3 ms is possible but impractical. - time = time < MIN_INTEGRATION_TIME ? MIN_INTEGRATION_TIME : time; - // also boundary limit max integration time too - time = time > MAX_INTEGRATION_TIME ? MAX_INTEGRATION_TIME : time; - - vexDeviceOpticalIntegrationTimeSet(device->device_info, time); - return_port(port - 1, PROS_SUCCESS); + claim_port_i(port - 1, E_DEVICE_OPTICAL); + // going to set the time to minimum of 3 ms, 3 ms is possible but impractical. + time = time < MIN_INTEGRATION_TIME ? MIN_INTEGRATION_TIME : time; + // also boundary limit max integration time too + time = time > MAX_INTEGRATION_TIME ? MAX_INTEGRATION_TIME : time; + + vexDeviceOpticalIntegrationTimeSet(device->device_info, time); + return_port(port - 1, PROS_SUCCESS); } \ No newline at end of file diff --git a/src/devices/vdml_optical.cpp b/src/devices/vdml_optical.cpp index b6fbc5ac..478bbff0 100644 --- a/src/devices/vdml_optical.cpp +++ b/src/devices/vdml_optical.cpp @@ -12,97 +12,98 @@ #include "pros/optical.h" #include "pros/optical.hpp" -#include "vdml/vdml.h" +#include "vdml/vdml.hpp" namespace pros { inline namespace v5 { using namespace pros::c; -Optical::Optical(std::uint8_t port) : Device(port, DeviceType::optical) {} +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; + 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); +double Optical::get_hue() { + return optical_get_hue(_port); } -double Optical::get_saturation(){ - return optical_get_saturation(_port); +double Optical::get_saturation() { + return optical_get_saturation(_port); } -double Optical::get_brightness(){ - return optical_get_brightness(_port); +double Optical::get_brightness() { + return optical_get_brightness(_port); } -std::int32_t Optical::get_proximity(){ - return optical_get_proximity(_port); +std::int32_t Optical::get_proximity() { + return optical_get_proximity(_port); } -std::int32_t Optical::set_led_pwm(std::uint8_t value){ - return optical_set_led_pwm(_port, value); +std::int32_t Optical::set_led_pwm(std::uint8_t value) { + return optical_set_led_pwm(_port, value); } -std::int32_t Optical::get_led_pwm(){ - return optical_get_led_pwm(_port); +std::int32_t Optical::get_led_pwm() { + return optical_get_led_pwm(_port); } -optical_rgb_s_t Optical::get_rgb(){ - return optical_get_rgb(_port); +optical_rgb_s_t Optical::get_rgb() { + return optical_get_rgb(_port); } -optical_raw_s_t Optical::get_raw(){ - return optical_get_raw(_port); +optical_raw_s_t Optical::get_raw() { + return optical_get_raw(_port); } -optical_direction_e_t Optical::get_gesture(){ - return optical_get_gesture(_port); +optical_direction_e_t Optical::get_gesture() { + return optical_get_gesture(_port); } -optical_gesture_s_t Optical::get_gesture_raw(){ - return optical_get_gesture_raw(_port); +optical_gesture_s_t Optical::get_gesture_raw() { + return optical_get_gesture_raw(_port); } -std::int32_t Optical::enable_gesture(){ - return optical_enable_gesture(_port); +std::int32_t Optical::enable_gesture() { + return optical_enable_gesture(_port); } -std::int32_t Optical::disable_gesture(){ - return optical_disable_gesture(_port); +std::int32_t Optical::disable_gesture() { + return optical_disable_gesture(_port); } double Optical::get_integration_time() { - return optical_get_integration_time(_port); + return optical_get_integration_time(_port); } std::int32_t Optical::set_integration_time(double time) { - return optical_set_integration_time(_port, time); + return optical_set_integration_time(_port, time); } std::ostream& operator<<(std::ostream& os, pros::Optical& optical) { - pros::c::optical_rgb_s_t rgb = optical.get_rgb(); - os << "Optical ["; - os << "port: " << optical.get_port(); - os << ", hue: " << optical.get_hue(); - os << ", saturation: " << optical.get_saturation(); - os << ", brightness: " << optical.get_brightness(); - os << ", proximity: " << optical.get_proximity(); - os << ", rgb: " << "{" << rgb.red << ","<< rgb.green << "," << rgb.blue << "}"; - os << "]"; - return os; + pros::c::optical_rgb_s_t rgb = optical.get_rgb(); + os << "Optical ["; + os << "port: " << optical.get_port(); + os << ", hue: " << optical.get_hue(); + os << ", saturation: " << optical.get_saturation(); + os << ", brightness: " << optical.get_brightness(); + os << ", proximity: " << optical.get_proximity(); + os << ", rgb: " << "{" << rgb.red << "," << rgb.green << "," << rgb.blue << "}"; + os << "]"; + return os; } namespace literals { const pros::Optical operator"" _opt(const unsigned long long int o) { - return pros::Optical(o); + return pros::Optical(o); } -} // +} // namespace literals } // namespace v5 } // namespace pros diff --git a/src/devices/vdml_rotation.c b/src/devices/vdml_rotation.c index 27761fe7..356188bc 100644 --- a/src/devices/vdml_rotation.c +++ b/src/devices/vdml_rotation.c @@ -10,73 +10,73 @@ * file, You can obtain one at http://mozilla.org/MPL/2.0/. */ -#include - #include "pros/rotation.h" #include "v5_api.h" -#include "vdml/registry.h" -#include "vdml/vdml.h" +#include "vdml/registry.hpp" +#include "vdml/vdml.hpp" + +#include #define ROTATION_RESET_TIMEOUT 1000 int32_t rotation_reset(uint8_t port) { - claim_port_i(port - 1, E_DEVICE_ROTATION); - vexDeviceAbsEncReset(device->device_info); - return_port(port - 1, PROS_SUCCESS); + claim_port_i(port - 1, E_DEVICE_ROTATION); + vexDeviceAbsEncReset(device->device_info); + return_port(port - 1, PROS_SUCCESS); } int32_t rotation_set_data_rate(uint8_t port, uint32_t rate) { - claim_port_i(port - 1, E_DEVICE_ROTATION); + 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) { - rate = ROTATION_MINIMUM_DATA_RATE; - } else { - rate -= rate % ROTATION_MINIMUM_DATA_RATE; - } + // rate is not less than 5ms, and rounded down to nearest increment of 5 + if (rate < ROTATION_MINIMUM_DATA_RATE) { + rate = ROTATION_MINIMUM_DATA_RATE; + } else { + rate -= rate % ROTATION_MINIMUM_DATA_RATE; + } - vexDeviceAbsEncDataRateSet(device->device_info, rate); - return_port(port - 1, PROS_SUCCESS); + vexDeviceAbsEncDataRateSet(device->device_info, rate); + return_port(port - 1, PROS_SUCCESS); } int32_t rotation_reset_position(uint8_t port) { - claim_port_i(port - 1, E_DEVICE_ROTATION); - vexDeviceAbsEncPositionSet(device->device_info, 0); - return_port(port - 1, PROS_SUCCESS); + claim_port_i(port - 1, E_DEVICE_ROTATION); + vexDeviceAbsEncPositionSet(device->device_info, 0); + return_port(port - 1, PROS_SUCCESS); } int32_t rotation_set_position(uint8_t port, int32_t position) { - claim_port_i(port - 1, E_DEVICE_ROTATION); - vexDeviceAbsEncPositionSet(device->device_info, position); - return_port(port - 1, PROS_SUCCESS); + claim_port_i(port - 1, E_DEVICE_ROTATION); + vexDeviceAbsEncPositionSet(device->device_info, position); + return_port(port - 1, PROS_SUCCESS); } int32_t rotation_get_position(uint8_t port) { - claim_port_i(port - 1, E_DEVICE_ROTATION); - int32_t rtn = vexDeviceAbsEncPositionGet(device->device_info); - return_port(port - 1, rtn); + claim_port_i(port - 1, E_DEVICE_ROTATION); + int32_t rtn = vexDeviceAbsEncPositionGet(device->device_info); + return_port(port - 1, rtn); } int32_t rotation_get_velocity(uint8_t port) { - claim_port_i(port - 1, E_DEVICE_ROTATION); - int32_t rtn = vexDeviceAbsEncVelocityGet(device->device_info); - return_port(port - 1, rtn); + claim_port_i(port - 1, E_DEVICE_ROTATION); + int32_t rtn = vexDeviceAbsEncVelocityGet(device->device_info); + return_port(port - 1, rtn); } int32_t rotation_get_angle(uint8_t port) { - claim_port_i(port - 1, E_DEVICE_ROTATION); - int32_t rtn = vexDeviceAbsEncAngleGet(device->device_info); - return_port(port - 1, rtn); + claim_port_i(port - 1, E_DEVICE_ROTATION); + int32_t rtn = vexDeviceAbsEncAngleGet(device->device_info); + return_port(port - 1, rtn); } int32_t rotation_set_reversed(uint8_t port, bool value) { - claim_port_i(port - 1, E_DEVICE_ROTATION); - vexDeviceAbsEncReverseFlagSet(device->device_info, value); - return_port(port - 1, PROS_SUCCESS); + claim_port_i(port - 1, E_DEVICE_ROTATION); + vexDeviceAbsEncReverseFlagSet(device->device_info, value); + return_port(port - 1, PROS_SUCCESS); } int32_t rotation_init_reverse(uint8_t port, bool reverse_flag) { - claim_port_i(port - 1, E_DEVICE_ROTATION); + claim_port_i(port - 1, E_DEVICE_ROTATION); uint16_t timeoutCount = 0; // releasing mutex so vexBackgrounProcessing can run without being blocked. do { @@ -90,19 +90,22 @@ int32_t rotation_init_reverse(uint8_t port, bool reverse_flag) { return PROS_ERR; } device = device; // suppressing compiler warning - } while(vexDeviceAbsEncStatusGet(device->device_info) == 0); + } while (vexDeviceAbsEncStatusGet(device->device_info) == 0); vexAbsEncReverseFlagSet(port - 1, reverse_flag); return_port(port - 1, 1) } -int32_t rotation_reverse(uint8_t port){ - claim_port_i(port - 1, E_DEVICE_ROTATION); - vexDeviceAbsEncReverseFlagSet(device->device_info, !vexDeviceAbsEncReverseFlagGet(device->device_info)); - return_port(port - 1, PROS_SUCCESS); +int32_t rotation_reverse(uint8_t port) { + claim_port_i(port - 1, E_DEVICE_ROTATION); + vexDeviceAbsEncReverseFlagSet( + device->device_info, + !vexDeviceAbsEncReverseFlagGet(device->device_info) + ); + return_port(port - 1, PROS_SUCCESS); } int32_t rotation_get_reversed(uint8_t port) { - claim_port_i(port - 1, E_DEVICE_ROTATION); - int32_t rtn = vexDeviceAbsEncReverseFlagGet(device->device_info); - return_port(port - 1, rtn); + claim_port_i(port - 1, E_DEVICE_ROTATION); + int32_t rtn = vexDeviceAbsEncReverseFlagGet(device->device_info); + return_port(port - 1, rtn); } \ No newline at end of file diff --git a/src/devices/vdml_rotation.cpp b/src/devices/vdml_rotation.cpp index ef7c88da..ddd8a7ee 100644 --- a/src/devices/vdml_rotation.cpp +++ b/src/devices/vdml_rotation.cpp @@ -11,84 +11,85 @@ */ #include "pros/rotation.hpp" -#include "vdml/vdml.h" +#include "vdml/vdml.hpp" namespace pros { inline namespace v5 { -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); - } +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() { - return pros::c::rotation_reset(_port); + return pros::c::rotation_reset(_port); } std::int32_t Rotation::set_data_rate(std::uint32_t rate) const { - return pros::c::rotation_set_data_rate(_port, rate); + return pros::c::rotation_set_data_rate(_port, rate); } std::int32_t Rotation::set_position(std::int32_t position) const { - return pros::c::rotation_set_position(_port, position); + return pros::c::rotation_set_position(_port, position); } std::int32_t Rotation::reset_position(void) const { - return pros::c::rotation_reset_position(_port); + 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::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); + return pros::c::rotation_get_position(_port); } std::int32_t Rotation::get_velocity(void) const { - return pros::c::rotation_get_velocity(_port); + return pros::c::rotation_get_velocity(_port); } std::int32_t Rotation::get_angle(void) const { - return pros::c::rotation_get_angle(_port); + return pros::c::rotation_get_angle(_port); } std::int32_t Rotation::set_reversed(bool value) const { - return pros::c::rotation_set_reversed(_port, value); + return pros::c::rotation_set_reversed(_port, value); } std::int32_t Rotation::reverse(void) const { - return pros::c::rotation_reverse(_port); + return pros::c::rotation_reverse(_port); } std::int32_t Rotation::get_reversed(void) const { - return pros::c::rotation_get_reversed(_port); + return pros::c::rotation_get_reversed(_port); } std::ostream& operator<<(std::ostream& os, const pros::Rotation& rotation) { - os << "Rotation ["; - os << "port: " << rotation._port; - os << ", position: " << rotation.get_position(); - os << ", velocity: " << rotation.get_velocity(); - os << ", angle: " << rotation.get_angle(); - os << ", reversed: " << rotation.get_reversed(); - os << "]"; - return os; + os << "Rotation ["; + os << "port: " << rotation._port; + os << ", position: " << rotation.get_position(); + os << ", velocity: " << rotation.get_velocity(); + os << ", angle: " << rotation.get_angle(); + os << ", reversed: " << rotation.get_reversed(); + os << "]"; + return os; } 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 +} // namespace v5 +} // namespace pros diff --git a/src/devices/vdml_serial.c b/src/devices/vdml_serial.c index a6cec624..7b30acc1 100644 --- a/src/devices/vdml_serial.c +++ b/src/devices/vdml_serial.c @@ -10,102 +10,102 @@ * file, You can obtain one at http://mozilla.org/MPL/2.0/. */ -#include -#include -#include - #include "kapi.h" #include "pros/serial.h" #include "v5_api.h" -#include "vdml/registry.h" -#include "vdml/vdml.h" +#include "vdml/registry.hpp" +#include "vdml/vdml.hpp" + +#include +#include +#include // Control function int32_t serial_enable(uint8_t port) { - /** - * claim_port_i(port - 1, E_DEVICE_SERIAL) is not used because it requires - * the port to already be of the requested type in VEXos, which will not yet - * be the case for generic serial as vexDeviceGenericSerialEnable is what - * switches the port into the correct mode - */ - if (!VALIDATE_PORT_NO(port - 1)) { - errno = EINVAL; - return PROS_ERR; - } - v5_smart_device_s_t* device = registry_get_device(port - 1); - if (!port_mutex_take(port - 1)) { - errno = EACCES; - return PROS_ERR; - } - vexDeviceGenericSerialEnable(device->device_info, 0); - return_port(port - 1, PROS_SUCCESS); + /** + * claim_port_i(port - 1, E_DEVICE_SERIAL) is not used because it requires + * the port to already be of the requested type in VEXos, which will not yet + * be the case for generic serial as vexDeviceGenericSerialEnable is what + * switches the port into the correct mode + */ + if (!VALIDATE_PORT_NO(port - 1)) { + errno = EINVAL; + return PROS_ERR; + } + v5_smart_device_s_t* device = registry_get_device(port - 1); + if (!port_mutex_take(port - 1)) { + errno = EACCES; + return PROS_ERR; + } + vexDeviceGenericSerialEnable(device->device_info, 0); + return_port(port - 1, PROS_SUCCESS); } int32_t serial_set_baudrate(uint8_t port, int32_t baudrate) { - claim_port_i(port - 1, E_DEVICE_SERIAL); - vexDeviceGenericSerialBaudrate(device->device_info, baudrate); - return_port(port - 1, PROS_SUCCESS); + claim_port_i(port - 1, E_DEVICE_SERIAL); + vexDeviceGenericSerialBaudrate(device->device_info, baudrate); + return_port(port - 1, PROS_SUCCESS); } int32_t serial_flush(uint8_t port) { - claim_port_i(port - 1, E_DEVICE_SERIAL); - vexDeviceGenericSerialFlush(device->device_info); - return_port(port - 1, PROS_SUCCESS); + claim_port_i(port - 1, E_DEVICE_SERIAL); + vexDeviceGenericSerialFlush(device->device_info); + return_port(port - 1, PROS_SUCCESS); } // Telemetry functions int32_t serial_get_read_avail(uint8_t port) { - claim_port_i(port - 1, E_DEVICE_SERIAL); - int32_t rtn = vexDeviceGenericSerialReceiveAvail(device->device_info); - return_port(port - 1, rtn); + claim_port_i(port - 1, E_DEVICE_SERIAL); + int32_t rtn = vexDeviceGenericSerialReceiveAvail(device->device_info); + return_port(port - 1, rtn); } int32_t serial_get_write_free(uint8_t port) { - claim_port_i(port - 1, E_DEVICE_SERIAL); - int32_t rtn = vexDeviceGenericSerialWriteFree(device->device_info); - return_port(port - 1, rtn); + claim_port_i(port - 1, E_DEVICE_SERIAL); + int32_t rtn = vexDeviceGenericSerialWriteFree(device->device_info); + return_port(port - 1, rtn); } // Read functions int32_t serial_peek_byte(uint8_t port) { - claim_port_i(port - 1, E_DEVICE_SERIAL); - int32_t rtn = vexDeviceGenericSerialPeekChar(device->device_info); - return_port(port - 1, rtn); + claim_port_i(port - 1, E_DEVICE_SERIAL); + int32_t rtn = vexDeviceGenericSerialPeekChar(device->device_info); + return_port(port - 1, rtn); } int32_t serial_read_byte(uint8_t port) { - claim_port_i(port - 1, E_DEVICE_SERIAL); - int32_t rtn = vexDeviceGenericSerialReadChar(device->device_info); - return_port(port - 1, rtn); + claim_port_i(port - 1, E_DEVICE_SERIAL); + int32_t rtn = vexDeviceGenericSerialReadChar(device->device_info); + return_port(port - 1, rtn); } int32_t serial_read(uint8_t port, uint8_t* buffer, int32_t length) { - claim_port_i(port - 1, E_DEVICE_SERIAL); - int32_t rtn = vexDeviceGenericSerialReceive(device->device_info, buffer, length); - return_port(port - 1, rtn); + claim_port_i(port - 1, E_DEVICE_SERIAL); + int32_t rtn = vexDeviceGenericSerialReceive(device->device_info, buffer, length); + return_port(port - 1, rtn); } // Write functions int32_t serial_write_byte(uint8_t port, uint8_t buffer) { - claim_port_i(port - 1, E_DEVICE_SERIAL); - int32_t rtn = vexDeviceGenericSerialWriteChar(device->device_info, buffer); - if (rtn == -1) { - errno = EIO; - return_port(port - 1, PROS_ERR); - } - return_port(port - 1, rtn); + claim_port_i(port - 1, E_DEVICE_SERIAL); + int32_t rtn = vexDeviceGenericSerialWriteChar(device->device_info, buffer); + if (rtn == -1) { + errno = EIO; + return_port(port - 1, PROS_ERR); + } + return_port(port - 1, rtn); } int32_t serial_write(uint8_t port, uint8_t* buffer, int32_t length) { - claim_port_i(port - 1, E_DEVICE_SERIAL); - int32_t rtn = vexDeviceGenericSerialTransmit(device->device_info, buffer, length); - if (rtn == -1) { - errno = EIO; - return_port(port - 1, PROS_ERR); - } - return_port(port - 1, rtn); + claim_port_i(port - 1, E_DEVICE_SERIAL); + int32_t rtn = vexDeviceGenericSerialTransmit(device->device_info, buffer, length); + if (rtn == -1) { + errno = EIO; + return_port(port - 1, PROS_ERR); + } + return_port(port - 1, rtn); } diff --git a/src/devices/vdml_serial.cpp b/src/devices/vdml_serial.cpp index 7e1ed79f..6c717507 100644 --- a/src/devices/vdml_serial.cpp +++ b/src/devices/vdml_serial.cpp @@ -12,59 +12,61 @@ #include "kapi.h" #include "pros/serial.hpp" -#include "vdml/vdml.h" +#include "vdml/vdml.hpp" namespace pros { using namespace pros::c; -Serial::Serial(std::uint8_t port, std::int32_t baudrate) : Device(port, DeviceType::serial) { - serial_enable(port); - set_baudrate(baudrate); +Serial::Serial(std::uint8_t port, std::int32_t baudrate) + : Device(port, DeviceType::serial) { + serial_enable(port); + set_baudrate(baudrate); } -Serial::Serial(std::uint8_t port) : Device(port, DeviceType::serial) { - serial_enable(port); +Serial::Serial(std::uint8_t port) + : Device(port, DeviceType::serial) { + serial_enable(port); } std::int32_t Serial::set_baudrate(std::int32_t baudrate) const { - return serial_set_baudrate(_port, baudrate); + return serial_set_baudrate(_port, baudrate); } std::int32_t Serial::flush() const { - return serial_flush(_port); + return serial_flush(_port); } std::int32_t Serial::get_read_avail() const { - return serial_get_read_avail(_port); + return serial_get_read_avail(_port); } std::int32_t Serial::get_write_free() const { - return serial_get_write_free(_port); + return serial_get_write_free(_port); } std::int32_t Serial::peek_byte() const { - return serial_peek_byte(_port); + return serial_peek_byte(_port); } std::int32_t Serial::read_byte() const { - return serial_read_byte(_port); + return serial_read_byte(_port); } std::int32_t Serial::read(std::uint8_t* buffer, std::int32_t length) const { - return serial_read(_port, buffer, length); + return serial_read(_port, buffer, length); } std::int32_t Serial::write_byte(std::uint8_t buffer) const { - return serial_write_byte(_port, buffer); + return serial_write_byte(_port, buffer); } std::int32_t Serial::write(std::uint8_t* buffer, std::int32_t length) const { - return serial_write(_port, buffer, length); + return serial_write(_port, buffer, length); } namespace literals { const pros::Serial operator"" _ser(const unsigned long long int m) { - return pros::Serial(m); + return pros::Serial(m); } -} // namespace literals -} // namespace pros +} // namespace literals +} // namespace pros diff --git a/src/devices/vdml_vision.c b/src/devices/vdml_vision.c index cc951de6..c174f149 100644 --- a/src/devices/vdml_vision.c +++ b/src/devices/vdml_vision.c @@ -13,361 +13,432 @@ #include "kapi.h" #include "v5_api.h" #include "v5_apitypes.h" -#include "vdml/registry.h" -#include "vdml/vdml.h" +#include "vdml/registry.hpp" +#include "vdml/vdml.hpp" typedef struct vision_data { - vision_zero_e_t zero_point; + vision_zero_e_t zero_point; } vision_data_s_t; static vision_zero_e_t get_zero_point(uint8_t port) { - return ((vision_data_s_t*)registry_get_device(port)->pad)->zero_point; + return ((vision_data_s_t*)registry_get_device(port)->pad)->zero_point; } static void set_zero_point(uint8_t port, vision_zero_e_t zero_point) { - vision_data_s_t* data = (vision_data_s_t*)registry_get_device(port)->pad; - data->zero_point = zero_point; + vision_data_s_t* data = (vision_data_s_t*)registry_get_device(port)->pad; + data->zero_point = zero_point; } static void _vision_transform_coords(uint8_t port, vision_object_s_t* object_ptr) { - switch (get_zero_point(port)) { - case E_VISION_ZERO_CENTER: - object_ptr->left_coord -= VISION_FOV_WIDTH / 2; - object_ptr->top_coord = (VISION_FOV_HEIGHT / 2) - object_ptr->top_coord; - break; - default: - break; - } - object_ptr->x_middle_coord = object_ptr->left_coord + (object_ptr->width / 2); - object_ptr->y_middle_coord = object_ptr->top_coord - (object_ptr->height / 2); + switch (get_zero_point(port)) { + case E_VISION_ZERO_CENTER: + object_ptr->left_coord -= VISION_FOV_WIDTH / 2; + object_ptr->top_coord = (VISION_FOV_HEIGHT / 2) - object_ptr->top_coord; + break; + default: + break; + } + object_ptr->x_middle_coord = object_ptr->left_coord + (object_ptr->width / 2); + object_ptr->y_middle_coord = object_ptr->top_coord - (object_ptr->height / 2); } int32_t vision_get_object_count(uint8_t port) { - claim_port_i(port - 1, E_DEVICE_VISION); - int32_t rtn = vexDeviceVisionObjectCountGet(device->device_info); - return_port(port - 1, rtn); + claim_port_i(port - 1, E_DEVICE_VISION); + int32_t rtn = vexDeviceVisionObjectCountGet(device->device_info); + return_port(port - 1, rtn); } vision_object_s_t vision_get_by_size(uint8_t port, const uint32_t size_id) { - vision_object_s_t rtn; - v5_smart_device_s_t* device; - if (!claim_port_try(port - 1, E_DEVICE_VISION)) { - rtn.signature = VISION_OBJECT_ERR_SIG; - return rtn; - } - device = registry_get_device(port - 1); - if ((uint32_t)vexDeviceVisionObjectCountGet(device->device_info) <= size_id) { - errno = EDOM; - rtn.signature = VISION_OBJECT_ERR_SIG; - goto leave; - } - if (vexDeviceVisionObjectGet(device->device_info, size_id, (V5_DeviceVisionObject*)&rtn) == 0) { - errno = EAGAIN; - rtn.signature = VISION_OBJECT_ERR_SIG; - goto leave; - } - _vision_transform_coords(port - 1, &rtn); + vision_object_s_t rtn; + v5_smart_device_s_t* device; + if (!claim_port_try(port - 1, E_DEVICE_VISION)) { + rtn.signature = VISION_OBJECT_ERR_SIG; + return rtn; + } + device = registry_get_device(port - 1); + if ((uint32_t)vexDeviceVisionObjectCountGet(device->device_info) <= size_id) { + errno = EDOM; + rtn.signature = VISION_OBJECT_ERR_SIG; + goto leave; + } + if (vexDeviceVisionObjectGet(device->device_info, size_id, (V5_DeviceVisionObject*)&rtn) == 0) { + errno = EAGAIN; + rtn.signature = VISION_OBJECT_ERR_SIG; + goto leave; + } + _vision_transform_coords(port - 1, &rtn); leave: - port_mutex_give(port - 1); - return rtn; + port_mutex_give(port - 1); + return rtn; } vision_object_s_t _vision_get_by_sig(uint8_t port, const uint32_t size_id, const uint32_t sig_id) { - vision_object_s_t rtn; - rtn.signature = VISION_OBJECT_ERR_SIG; - v5_smart_device_s_t* device; - uint8_t count = 0; - int32_t object_count = 0; - - if (!claim_port_try(port - 1, E_DEVICE_VISION)) { - goto err_return_no_mutex; - } - - device = registry_get_device(port - 1); - object_count = vexDeviceVisionObjectCountGet(device->device_info); - if ((uint32_t)object_count <= size_id) { - errno = EDOM; - goto err_return; - } - - for (uint8_t i = 0; i <= object_count; i++) { - vision_object_s_t check; - if (vexDeviceVisionObjectGet(device->device_info, i, (V5_DeviceVisionObject*)&check) == PROS_ERR) { - errno = EAGAIN; - rtn = check; - goto err_return; - } - if (check.signature == sig_id) { - if (count == size_id) { - rtn = check; - _vision_transform_coords(port - 1, &rtn); - port_mutex_give(port - 1); - return rtn; - } - count++; - } - } - errno = EDOM; // we read through all the objects and none matched sig_id and size_id + vision_object_s_t rtn; + rtn.signature = VISION_OBJECT_ERR_SIG; + v5_smart_device_s_t* device; + uint8_t count = 0; + int32_t object_count = 0; + + if (!claim_port_try(port - 1, E_DEVICE_VISION)) { + goto err_return_no_mutex; + } + + device = registry_get_device(port - 1); + object_count = vexDeviceVisionObjectCountGet(device->device_info); + if ((uint32_t)object_count <= size_id) { + errno = EDOM; + goto err_return; + } + + for (uint8_t i = 0; i <= object_count; i++) { + vision_object_s_t check; + if (vexDeviceVisionObjectGet(device->device_info, i, (V5_DeviceVisionObject*)&check) + == PROS_ERR) { + errno = EAGAIN; + rtn = check; + goto err_return; + } + if (check.signature == sig_id) { + if (count == size_id) { + rtn = check; + _vision_transform_coords(port - 1, &rtn); + port_mutex_give(port - 1); + return rtn; + } + count++; + } + } + errno = EDOM; // we read through all the objects and none matched sig_id and size_id err_return: - port_mutex_give(port - 1); + port_mutex_give(port - 1); err_return_no_mutex: - rtn.signature = VISION_OBJECT_ERR_SIG; - return rtn; + rtn.signature = VISION_OBJECT_ERR_SIG; + return rtn; } vision_object_s_t vision_get_by_sig(uint8_t port, const uint32_t size_id, const uint32_t sig_id) { - if (sig_id > 7 || sig_id == 0) { - errno = EINVAL; - vision_object_s_t rtn; - rtn.signature = VISION_OBJECT_ERR_SIG; - return rtn; - } - return _vision_get_by_sig(port, size_id, sig_id); + if (sig_id > 7 || sig_id == 0) { + errno = EINVAL; + vision_object_s_t rtn; + rtn.signature = VISION_OBJECT_ERR_SIG; + return rtn; + } + return _vision_get_by_sig(port, size_id, sig_id); } -vision_object_s_t vision_get_by_code(uint8_t port, const uint32_t size_id, const vision_color_code_t color_code) { - return _vision_get_by_sig(port, size_id, color_code); +vision_object_s_t +vision_get_by_code(uint8_t port, const uint32_t size_id, const vision_color_code_t color_code) { + return _vision_get_by_sig(port, size_id, color_code); } -int32_t vision_read_by_size(uint8_t port, const uint32_t size_id, const uint32_t object_count, - vision_object_s_t* const object_arr) { - claim_port_i(port - 1, E_DEVICE_VISION); - for (uint8_t i = 0; i < object_count; i++) { - object_arr[i].signature = VISION_OBJECT_ERR_SIG; - } - uint32_t c = vexDeviceVisionObjectCountGet(device->device_info); - if (c <= size_id) { - port_mutex_give(port - 1); - errno = EDOM; - return PROS_ERR; - } else if (c > object_count + size_id) { - c = object_count + size_id; - } - - for (uint32_t i = size_id; i < c; i++) { - if (!vexDeviceVisionObjectGet(device->device_info, i, (V5_DeviceVisionObject*)(object_arr + i))) { - errno = EAGAIN; - object_arr[i].signature = VISION_OBJECT_ERR_SIG; - break; - } - _vision_transform_coords(port - 1, &object_arr[i]); - } - return_port(port - 1, c); +int32_t vision_read_by_size( + uint8_t port, + const uint32_t size_id, + const uint32_t object_count, + vision_object_s_t* const object_arr +) { + claim_port_i(port - 1, E_DEVICE_VISION); + for (uint8_t i = 0; i < object_count; i++) { + object_arr[i].signature = VISION_OBJECT_ERR_SIG; + } + uint32_t c = vexDeviceVisionObjectCountGet(device->device_info); + if (c <= size_id) { + port_mutex_give(port - 1); + errno = EDOM; + return PROS_ERR; + } else if (c > object_count + size_id) { + c = object_count + size_id; + } + + for (uint32_t i = size_id; i < c; i++) { + if (!vexDeviceVisionObjectGet( + device->device_info, + i, + (V5_DeviceVisionObject*)(object_arr + i) + )) { + errno = EAGAIN; + object_arr[i].signature = VISION_OBJECT_ERR_SIG; + break; + } + _vision_transform_coords(port - 1, &object_arr[i]); + } + return_port(port - 1, c); } -int32_t _vision_read_by_sig(uint8_t port, const uint32_t size_id, const uint32_t sig_id, const uint32_t object_count, - vision_object_s_t* const object_arr) { - claim_port_i(port - 1, E_DEVICE_VISION); - for (uint8_t i = 0; i < object_count; i++) { - object_arr[i].signature = VISION_OBJECT_ERR_SIG; - } - uint32_t c = vexDeviceVisionObjectCountGet(device->device_info); - if (c <= size_id) { - errno = EDOM; - port_mutex_give(port - 1); - return PROS_ERR; - } - if (c > object_count) { - c = object_count; - } - - uint32_t j = 0; // track how many objects we've placed into object_arr - uint32_t seen = 0; // track how many objects we've seen matching sig_id - for (uint8_t i = 0; i < c; i++) { // loop through all objects on sensor - // place i-th object on vision sensor on j-th position in object_arr - if (!vexDeviceVisionObjectGet(device->device_info, i, (V5_DeviceVisionObject*)(object_arr + j))) { - errno = EAGAIN; - object_arr[i].signature = VISION_OBJECT_ERR_SIG; - goto rtn; - } - // check if this (j-th) object matches sig_id - if (object_arr[j].signature == sig_id) { - seen++; - if (seen >= size_id) { // have we seen enough objects to start adding to object_arr? - // if so, transform the coords and "commit" it by incrementing j - _vision_transform_coords(port - 1, &object_arr[j]); - j++; - if (j == object_count) goto rtn; - } - } - } - errno = EDOM; // read through all objects and couldn't find enough objects matching filter parameters +int32_t _vision_read_by_sig( + uint8_t port, + const uint32_t size_id, + const uint32_t sig_id, + const uint32_t object_count, + vision_object_s_t* const object_arr +) { + claim_port_i(port - 1, E_DEVICE_VISION); + for (uint8_t i = 0; i < object_count; i++) { + object_arr[i].signature = VISION_OBJECT_ERR_SIG; + } + uint32_t c = vexDeviceVisionObjectCountGet(device->device_info); + if (c <= size_id) { + errno = EDOM; + port_mutex_give(port - 1); + return PROS_ERR; + } + if (c > object_count) { + c = object_count; + } + + uint32_t j = 0; // track how many objects we've placed into object_arr + uint32_t seen = 0; // track how many objects we've seen matching sig_id + for (uint8_t i = 0; i < c; i++) { // loop through all objects on sensor + // place i-th object on vision sensor on j-th position in object_arr + if (!vexDeviceVisionObjectGet( + device->device_info, + i, + (V5_DeviceVisionObject*)(object_arr + j) + )) { + errno = EAGAIN; + object_arr[i].signature = VISION_OBJECT_ERR_SIG; + goto rtn; + } + // check if this (j-th) object matches sig_id + if (object_arr[j].signature == sig_id) { + seen++; + if (seen >= size_id) { // have we seen enough objects to start adding to object_arr? + // if so, transform the coords and "commit" it by incrementing j + _vision_transform_coords(port - 1, &object_arr[j]); + j++; + if (j == object_count) + goto rtn; + } + } + } + errno = EDOM; // read through all objects and couldn't find enough objects matching filter + // parameters rtn: - return_port(port - 1, j); + return_port(port - 1, j); } -int32_t vision_read_by_sig(uint8_t port, const uint32_t size_id, const uint32_t sig_id, const uint32_t object_count, - vision_object_s_t* const object_arr) { - if (sig_id > 7 || sig_id == 0) { - errno = EINVAL; - for (uint8_t i = 0; i < object_count; i++) { - object_arr[i].signature = VISION_OBJECT_ERR_SIG; - } - return PROS_ERR; - } - return _vision_read_by_sig(port, size_id, sig_id, object_count, object_arr); +int32_t vision_read_by_sig( + uint8_t port, + const uint32_t size_id, + const uint32_t sig_id, + const uint32_t object_count, + vision_object_s_t* const object_arr +) { + if (sig_id > 7 || sig_id == 0) { + errno = EINVAL; + for (uint8_t i = 0; i < object_count; i++) { + object_arr[i].signature = VISION_OBJECT_ERR_SIG; + } + return PROS_ERR; + } + return _vision_read_by_sig(port, size_id, sig_id, object_count, object_arr); } -int32_t vision_read_by_code(uint8_t port, const uint32_t size_id, const vision_color_code_t color_code, - const uint32_t object_count, vision_object_s_t* const object_arr) { - return _vision_read_by_sig(port, size_id, color_code, object_count, object_arr); +int32_t vision_read_by_code( + uint8_t port, + const uint32_t size_id, + const vision_color_code_t color_code, + const uint32_t object_count, + vision_object_s_t* const object_arr +) { + return _vision_read_by_sig(port, size_id, color_code, object_count, object_arr); } vision_signature_s_t vision_get_signature(uint8_t port, const uint8_t signature_id) { - vision_signature_s_t sig; - sig.id = VISION_OBJECT_ERR_SIG; - if (signature_id > 7 || signature_id == 0) { - errno = EINVAL; - return sig; - } - int32_t rtn = claim_port_try(port - 1, E_DEVICE_VISION); - if (!rtn) { - return sig; - } - v5_smart_device_s_t* device = registry_get_device(port - 1); - rtn = vexDeviceVisionSignatureGet(device->device_info, signature_id, (V5_DeviceVisionSignature*)&sig); - if (!rtn || !sig._pad[0]) { // sig._pad[0] is flags, will be set to 1 if data is valid and signatures are sent - errno = EAGAIN; - sig.id = VISION_OBJECT_ERR_SIG; - } - port_mutex_give(port - 1); - return sig; + vision_signature_s_t sig; + sig.id = VISION_OBJECT_ERR_SIG; + if (signature_id > 7 || signature_id == 0) { + errno = EINVAL; + return sig; + } + int32_t rtn = claim_port_try(port - 1, E_DEVICE_VISION); + if (!rtn) { + return sig; + } + v5_smart_device_s_t* device = registry_get_device(port - 1); + rtn = vexDeviceVisionSignatureGet( + device->device_info, + signature_id, + (V5_DeviceVisionSignature*)&sig + ); + if (!rtn || !sig._pad[0]) { // sig._pad[0] is flags, will be set to 1 if data is valid and + // signatures are sent + errno = EAGAIN; + sig.id = VISION_OBJECT_ERR_SIG; + } + port_mutex_give(port - 1); + return sig; } -int32_t vision_set_signature(uint8_t port, const uint8_t signature_id, vision_signature_s_t* const signature_ptr) { - if (signature_id > 7 || signature_id == 0) { - errno = EINVAL; - return PROS_ERR; - } - signature_ptr->id = signature_id; - - claim_port_i(port - 1, E_DEVICE_VISION); - vexDeviceVisionSignatureSet(device->device_info, (V5_DeviceVisionSignature*)signature_ptr); - return_port(port - 1, PROS_SUCCESS); +int32_t vision_set_signature( + uint8_t port, + const uint8_t signature_id, + vision_signature_s_t* const signature_ptr +) { + if (signature_id > 7 || signature_id == 0) { + errno = EINVAL; + return PROS_ERR; + } + signature_ptr->id = signature_id; + + claim_port_i(port - 1, E_DEVICE_VISION); + vexDeviceVisionSignatureSet(device->device_info, (V5_DeviceVisionSignature*)signature_ptr); + return_port(port - 1, PROS_SUCCESS); } -vision_signature_s_t vision_signature_from_utility(const int32_t id, const int32_t u_min, const int32_t u_max, - const int32_t u_mean, const int32_t v_min, const int32_t v_max, - const int32_t v_mean, const float range, const int32_t type) { - vision_signature_s_t sig = {0}; - sig.id = id; - sig.range = range; - sig.u_min = u_min; - sig.u_max = u_max; - sig.u_mean = u_mean; - sig.v_min = v_min; - sig.v_max = v_max; - sig.v_mean = v_mean; - sig.type = type; - return sig; +vision_signature_s_t vision_signature_from_utility( + const int32_t id, + const int32_t u_min, + const int32_t u_max, + const int32_t u_mean, + const int32_t v_min, + const int32_t v_max, + const int32_t v_mean, + const float range, + const int32_t type +) { + vision_signature_s_t sig = {0}; + sig.id = id; + sig.range = range; + sig.u_min = u_min; + sig.u_max = u_max; + sig.u_mean = u_mean; + sig.v_min = v_min; + sig.v_max = v_max; + sig.v_mean = v_mean; + sig.type = type; + return sig; } -vision_color_code_t vision_create_color_code(uint8_t port, const uint32_t sig_id1, const uint32_t sig_id2, - const uint32_t sig_id3, const uint32_t sig_id4, const uint32_t sig_id5) { - uint16_t id = 0; - if (!sig_id1 || !sig_id2 || sig_id1 > 7 || sig_id2 > 7 || sig_id3 > 7 || sig_id4 > 7 || sig_id5 > 7) { - // Need to at least have two signatures to make a color code, and they all - // must be in the range [0-7] - errno = EINVAL; - id = VISION_OBJECT_ERR_SIG; - return id; - } - - const uint32_t sigs[5] = {sig_id1, sig_id2, sig_id3, sig_id4, sig_id5}; - for (size_t i = 0; i < 5 && sigs[i]; i++) { - register const uint32_t sig_id = sigs[i]; - id = (id << 3) | sig_id; - - vision_signature_s_t stored_sig = vision_get_signature(port, sig_id); - if (stored_sig.type != E_VISION_OBJECT_COLOR_CODE) { - stored_sig.type = E_VISION_OBJECT_COLOR_CODE; - vision_set_signature(port, sig_id, &stored_sig); - } - } - - return id; +vision_color_code_t vision_create_color_code( + uint8_t port, + const uint32_t sig_id1, + const uint32_t sig_id2, + const uint32_t sig_id3, + const uint32_t sig_id4, + const uint32_t sig_id5 +) { + uint16_t id = 0; + if (!sig_id1 || !sig_id2 || sig_id1 > 7 || sig_id2 > 7 || sig_id3 > 7 || sig_id4 > 7 + || sig_id5 > 7) { + // Need to at least have two signatures to make a color code, and they all + // must be in the range [0-7] + errno = EINVAL; + id = VISION_OBJECT_ERR_SIG; + return id; + } + + const uint32_t sigs[5] = {sig_id1, sig_id2, sig_id3, sig_id4, sig_id5}; + for (size_t i = 0; i < 5 && sigs[i]; i++) { + register const uint32_t sig_id = sigs[i]; + id = (id << 3) | sig_id; + + vision_signature_s_t stored_sig = vision_get_signature(port, sig_id); + if (stored_sig.type != E_VISION_OBJECT_COLOR_CODE) { + stored_sig.type = E_VISION_OBJECT_COLOR_CODE; + vision_set_signature(port, sig_id, &stored_sig); + } + } + + return id; } int32_t vision_set_led(uint8_t port, const int32_t rgb) { - claim_port_i(port - 1, E_DEVICE_VISION); - vexDeviceVisionLedModeSet(device->device_info, 1); - V5_DeviceVisionRgb _rgb = {.red = COLOR2R(rgb), .blue = COLOR2B(rgb), .green = COLOR2G(rgb), .brightness = 255}; - vexDeviceVisionLedColorSet(device->device_info, _rgb); - return_port(port - 1, PROS_SUCCESS); + claim_port_i(port - 1, E_DEVICE_VISION); + vexDeviceVisionLedModeSet(device->device_info, 1); + V5_DeviceVisionRgb _rgb = + {.red = COLOR2R(rgb), .blue = COLOR2B(rgb), .green = COLOR2G(rgb), .brightness = 255}; + vexDeviceVisionLedColorSet(device->device_info, _rgb); + return_port(port - 1, PROS_SUCCESS); } int32_t vision_clear_led(uint8_t port) { - claim_port_i(port - 1, E_DEVICE_VISION); - vexDeviceVisionLedModeSet(device->device_info, 0); - return_port(port - 1, PROS_SUCCESS); + claim_port_i(port - 1, E_DEVICE_VISION); + vexDeviceVisionLedModeSet(device->device_info, 0); + return_port(port - 1, PROS_SUCCESS); } int32_t vision_set_exposure(uint8_t port, const uint8_t percent) { - claim_port_i(port - 1, E_DEVICE_VISION); - // This translation comes from VEX to match the brightness represented in vision utility - vexDeviceVisionBrightnessSet(device->device_info, (((int)((percent * 100) + 50)) / 255)); - return_port(port - 1, PROS_SUCCESS); + claim_port_i(port - 1, E_DEVICE_VISION); + // This translation comes from VEX to match the brightness represented in vision utility + vexDeviceVisionBrightnessSet(device->device_info, (((int)((percent * 100) + 50)) / 255)); + return_port(port - 1, PROS_SUCCESS); } int32_t vision_get_exposure(uint8_t port) { - claim_port_i(port - 1, E_DEVICE_VISION); - // This translation comes from VEX to match the brightness represented in vision utility - int32_t rtn = ((vexDeviceVisionBrightnessGet(device->device_info) * 255) + 50) / 100; - return_port(port - 1, rtn); + claim_port_i(port - 1, E_DEVICE_VISION); + // This translation comes from VEX to match the brightness represented in vision utility + int32_t rtn = ((vexDeviceVisionBrightnessGet(device->device_info) * 255) + 50) / 100; + return_port(port - 1, rtn); } int32_t vision_set_auto_white_balance(uint8_t port, const uint8_t enable) { - if (enable != 0 && enable != 1) { - errno = EINVAL; - return PROS_ERR; - } - claim_port_i(port - 1, E_DEVICE_VISION); - vexDeviceVisionWhiteBalanceModeSet(device->device_info, enable + 1); - return_port(port - 1, PROS_SUCCESS); + if (enable != 0 && enable != 1) { + errno = EINVAL; + return PROS_ERR; + } + claim_port_i(port - 1, E_DEVICE_VISION); + vexDeviceVisionWhiteBalanceModeSet(device->device_info, enable + 1); + return_port(port - 1, PROS_SUCCESS); } int32_t vision_set_white_balance(uint8_t port, const int32_t rgb) { - claim_port_i(port - 1, E_DEVICE_VISION); - vexDeviceVisionWhiteBalanceModeSet(device->device_info, 2); - V5_DeviceVisionRgb _rgb = {.red = COLOR2R(rgb), .blue = COLOR2B(rgb), .green = COLOR2G(rgb), .brightness = 255}; - vexDeviceVisionWhiteBalanceSet(device->device_info, _rgb); - return_port(port - 1, PROS_SUCCESS); + claim_port_i(port - 1, E_DEVICE_VISION); + vexDeviceVisionWhiteBalanceModeSet(device->device_info, 2); + V5_DeviceVisionRgb _rgb = + {.red = COLOR2R(rgb), .blue = COLOR2B(rgb), .green = COLOR2G(rgb), .brightness = 255}; + vexDeviceVisionWhiteBalanceSet(device->device_info, _rgb); + return_port(port - 1, PROS_SUCCESS); } int32_t vision_get_white_balance(uint8_t port) { - claim_port_i(port - 1, E_DEVICE_VISION); - V5_DeviceVisionRgb rgb = vexDeviceVisionWhiteBalanceGet(device->device_info); - return_port(port - 1, RGB2COLOR(rgb.red, rgb.green, rgb.blue)); + claim_port_i(port - 1, E_DEVICE_VISION); + V5_DeviceVisionRgb rgb = vexDeviceVisionWhiteBalanceGet(device->device_info); + return_port(port - 1, RGB2COLOR(rgb.red, rgb.green, rgb.blue)); } int32_t vision_set_zero_point(uint8_t port, vision_zero_e_t zero_point) { - if (!VALIDATE_PORT_NO(port - 1)) { - errno = ENXIO; - return PROS_ERR; - } - if (registry_validate_binding(port - 1, E_DEVICE_VISION) != 0) { - errno = ENODEV; - return PROS_ERR; - } - if (!port_mutex_take(port - 1)) { - errno = EACCES; - return PROS_ERR; - } - set_zero_point(port - 1, zero_point); - return_port(port - 1, PROS_SUCCESS); + if (!VALIDATE_PORT_NO(port - 1)) { + errno = ENXIO; + return PROS_ERR; + } + if (registry_validate_binding(port - 1, E_DEVICE_VISION) != 0) { + errno = ENODEV; + return PROS_ERR; + } + if (!port_mutex_take(port - 1)) { + errno = EACCES; + return PROS_ERR; + } + set_zero_point(port - 1, zero_point); + return_port(port - 1, PROS_SUCCESS); } int32_t vision_set_wifi_mode(uint8_t port, const uint8_t enable) { - claim_port_i(port - 1, E_DEVICE_VISION); - vexDeviceVisionWifiModeSet(device->device_info, !!enable); - return_port(port - 1, PROS_SUCCESS); + claim_port_i(port - 1, E_DEVICE_VISION); + vexDeviceVisionWifiModeSet(device->device_info, !!enable); + return_port(port - 1, PROS_SUCCESS); } int32_t vision_print_signature(const vision_signature_s_t sig) { - printf("\n\npros::vision_signature_s_t SIG_%d = {", sig.id); - printf("%d, {%d, %d, %d}, %f, %ld, %ld, %ld, %ld, %ld, %ld, %ld, %ld};\n\n", sig.id, sig._pad[0], sig._pad[1], - sig._pad[2], sig.range, sig.u_min, sig.u_max, sig.u_mean, sig.v_min, sig.v_max, sig.v_mean, sig.rgb, sig.type); - return 1; + printf("\n\npros::vision_signature_s_t SIG_%d = {", sig.id); + printf( + "%d, {%d, %d, %d}, %f, %ld, %ld, %ld, %ld, %ld, %ld, %ld, %ld};\n\n", + sig.id, + sig._pad[0], + sig._pad[1], + sig._pad[2], + sig.range, + sig.u_min, + sig.u_max, + sig.u_mean, + sig.v_min, + sig.v_max, + sig.v_mean, + sig.rgb, + sig.type + ); + return 1; } diff --git a/src/devices/vdml_vision.cpp b/src/devices/vdml_vision.cpp index 138fff01..f58c6154 100644 --- a/src/devices/vdml_vision.cpp +++ b/src/devices/vdml_vision.cpp @@ -11,139 +11,177 @@ */ #include "kapi.h" -#include "vdml/vdml.h" +#include "vdml/vdml.hpp" namespace pros { inline namespace v5 { using namespace pros::c; -Vision::Vision(std::uint8_t port, vision_zero_e_t zero_point) : Device(port, DeviceType::vision) { - vision_set_zero_point(port, zero_point); +Vision::Vision(std::uint8_t port, vision_zero_e_t zero_point) + : Device(port, DeviceType::vision) { + vision_set_zero_point(port, zero_point); } std::int32_t Vision::clear_led(void) const { - return vision_clear_led(_port); -} - -vision_signature_s_t Vision::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, - const std::int32_t v_min, const std::int32_t v_max, - const std::int32_t v_mean, const float range, - const std::int32_t type) { - return c::vision_signature_from_utility(id, u_min, u_max, u_mean, v_min, v_max, v_mean, range, type); -} - -vision_color_code_t Vision::create_color_code(const std::uint32_t sig_id1, const std::uint32_t sig_id2, - const std::uint32_t sig_id3, const std::uint32_t sig_id4, - const std::uint32_t sig_id5) const { - return vision_create_color_code(_port, sig_id1, sig_id2, sig_id3, sig_id4, sig_id5); + return vision_clear_led(_port); +} + +vision_signature_s_t Vision::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, + const std::int32_t v_min, + const std::int32_t v_max, + const std::int32_t v_mean, + const float range, + const std::int32_t type +) { + return c::vision_signature_from_utility( + id, + u_min, + u_max, + u_mean, + v_min, + v_max, + v_mean, + range, + type + ); +} + +vision_color_code_t Vision::create_color_code( + const std::uint32_t sig_id1, + const std::uint32_t sig_id2, + const std::uint32_t sig_id3, + const std::uint32_t sig_id4, + const std::uint32_t sig_id5 +) 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 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; + 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); + return vision_get_by_size(_port, size_id); } -vision_object_s_t Vision::get_by_sig(const std::uint32_t size_id, const std::uint32_t sig_id) const { - return vision_get_by_sig(_port, size_id, sig_id); +vision_object_s_t +Vision::get_by_sig(const std::uint32_t size_id, const std::uint32_t sig_id) const { + return vision_get_by_sig(_port, size_id, sig_id); } -vision_object_s_t Vision::get_by_code(const std::uint32_t size_id, const vision_color_code_t color_code) const { - return vision_get_by_code(_port, size_id, color_code); +vision_object_s_t +Vision::get_by_code(const std::uint32_t size_id, const vision_color_code_t color_code) const { + return vision_get_by_code(_port, size_id, color_code); } int32_t Vision::get_exposure(void) const { - return vision_get_exposure(_port); + return vision_get_exposure(_port); } int32_t Vision::get_object_count(void) const { - return vision_get_object_count(_port); + return vision_get_object_count(_port); } std::int32_t Vision::get_white_balance(void) const { - return vision_get_white_balance(_port); + 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); +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); } -int32_t Vision::read_by_sig(const std::uint32_t size_id, const std::uint32_t sig_id, const std::uint32_t object_count, - vision_object_s_t* const object_arr) const { - return vision_read_by_sig(_port, size_id, sig_id, object_count, object_arr); +int32_t Vision::read_by_sig( + const std::uint32_t size_id, + const std::uint32_t sig_id, + const std::uint32_t object_count, + vision_object_s_t* const object_arr +) const { + return vision_read_by_sig(_port, size_id, sig_id, object_count, object_arr); } -int32_t Vision::read_by_code(const std::uint32_t size_id, const vision_color_code_t color_code, - const std::uint32_t object_count, vision_object_s_t* const object_arr) const { - return vision_read_by_code(_port, size_id, color_code, object_count, object_arr); +int32_t Vision::read_by_code( + const std::uint32_t size_id, + const vision_color_code_t color_code, + const std::uint32_t object_count, + vision_object_s_t* const object_arr +) const { + return vision_read_by_code(_port, size_id, color_code, object_count, object_arr); } vision_signature_s_t Vision::get_signature(const std::uint8_t signature_id) const { - return vision_get_signature(_port, signature_id); + return vision_get_signature(_port, signature_id); } std::int32_t Vision::print_signature(const vision_signature_s_t sig) { - return vision_print_signature(sig); + return vision_print_signature(sig); } -std::int32_t Vision::set_signature(const std::uint8_t signature_id, vision_signature_s_t* const signature_ptr) const { - return vision_set_signature(_port, signature_id, signature_ptr); +std::int32_t Vision::set_signature( + const std::uint8_t signature_id, + vision_signature_s_t* const signature_ptr +) const { + return vision_set_signature(_port, signature_id, signature_ptr); } std::int32_t Vision::set_auto_white_balance(const std::uint8_t enable) const { - return vision_set_auto_white_balance(_port, enable); + return vision_set_auto_white_balance(_port, enable); } std::int32_t Vision::set_exposure(const std::uint8_t exposure) const { - return vision_set_exposure(_port, exposure); + return vision_set_exposure(_port, exposure); } std::int32_t Vision::set_led(const std::int32_t rgb) const { - return vision_set_led(_port, rgb); + return vision_set_led(_port, rgb); } std::int32_t Vision::set_white_balance(const std::int32_t rgb) const { - return vision_set_white_balance(_port, rgb); + return vision_set_white_balance(_port, rgb); } std::int32_t Vision::set_zero_point(vision_zero_e_t zero_point) const { - return vision_set_zero_point(_port, zero_point); + return vision_set_zero_point(_port, zero_point); } std::int32_t Vision::set_wifi_mode(const std::uint8_t enable) const { - return vision_set_wifi_mode(_port, enable); + 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 + 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); + return Vision(m); } -} // namespace literals -} // namespace pros +} // namespace literals +} // namespace pros diff --git a/src/system/dev/dev_driver.c b/src/system/dev/dev_driver.c index 7d22a1c7..f3e56de5 100644 --- a/src/system/dev/dev_driver.c +++ b/src/system/dev/dev_driver.c @@ -14,12 +14,6 @@ * file, You can obtain one at http://mozilla.org/MPL/2.0/. */ -#include -#include -#include -#include -#include - #include "common/set.h" #include "common/string.h" #include "kapi.h" @@ -27,142 +21,150 @@ #include "system/dev/vfs.h" #include "system/optimizers.h" #include "v5_api.h" -#include "vdml/vdml.h" +#include "vdml/vdml.hpp" + +#include +#include +#include +#include +#include #define ASCII_ZERO 48 typedef struct dev_file_arg { - uint32_t port; - int flags; + uint32_t port; + int flags; } dev_file_arg_t; /******************************************************************************/ /** newlib driver functions **/ /******************************************************************************/ int dev_read_r(struct _reent* r, void* const arg, uint8_t* buffer, const size_t len) { - dev_file_arg_t* file_arg = (dev_file_arg_t*)arg; - uint32_t port = file_arg->port; - int32_t recv = 0; - while (true) { - recv = serial_read(port, (uint8_t*)(buffer + recv), len - recv); - if (recv == PROS_ERR) { - return 0; - } - if (file_arg->flags & O_NONBLOCK || recv >= 1) { - break; - } - task_delay(2); - } - if (recv == 0) { - errno = EAGAIN; - return 0; - } - return recv; + dev_file_arg_t* file_arg = (dev_file_arg_t*)arg; + uint32_t port = file_arg->port; + int32_t recv = 0; + while (true) { + recv = serial_read(port, (uint8_t*)(buffer + recv), len - recv); + if (recv == PROS_ERR) { + return 0; + } + if (file_arg->flags & O_NONBLOCK || recv >= 1) { + break; + } + task_delay(2); + } + if (recv == 0) { + errno = EAGAIN; + return 0; + } + return recv; } int dev_write_r(struct _reent* r, void* const arg, const uint8_t* buf, const size_t len) { - dev_file_arg_t* file_arg = (dev_file_arg_t*)arg; - uint32_t port = file_arg->port; - int32_t wrtn = 0; - while (true) { - int32_t w = serial_write(port, (uint8_t*)(buf + wrtn), len - wrtn); - if (w == PROS_ERR) { - return wrtn; - } - wrtn += w; - if (file_arg->flags & O_NONBLOCK || wrtn >= len) { - break; - } - task_delay(2); - } - if (wrtn == 0) { - errno = EAGAIN; - return 0; - } - return wrtn; + dev_file_arg_t* file_arg = (dev_file_arg_t*)arg; + uint32_t port = file_arg->port; + int32_t wrtn = 0; + while (true) { + int32_t w = serial_write(port, (uint8_t*)(buf + wrtn), len - wrtn); + if (w == PROS_ERR) { + return wrtn; + } + wrtn += w; + if (file_arg->flags & O_NONBLOCK || wrtn >= len) { + break; + } + task_delay(2); + } + if (wrtn == 0) { + errno = EAGAIN; + return 0; + } + return wrtn; } int dev_close_r(struct _reent* r, void* const arg) { - return 0; + return 0; } int dev_fstat_r(struct _reent* r, void* const arg, struct stat* st) { - // this is a complete implementation - st->st_mode = S_IFCHR; - return 0; + // this is a complete implementation + st->st_mode = S_IFCHR; + return 0; } int dev_isatty_r(struct _reent* r, void* const arg) { - // this is a complete implementation - return 0; + // this is a complete implementation + return 0; } off_t dev_lseek_r(struct _reent* r, void* const arg, off_t ptr, int dir) { - // lseek doesn't make sense on a serial line - r->_errno = ESPIPE; - return -1; + // lseek doesn't make sense on a serial line + r->_errno = ESPIPE; + return -1; } int dev_ctl(void* const arg, const uint32_t cmd, void* const extra_arg) { - dev_file_arg_t* file_arg = (dev_file_arg_t*)arg; - uint32_t port = file_arg->port; - switch (cmd) { - case DEVCTL_FIONREAD: - return serial_get_read_avail(port); - case DEVCTL_FIONWRITE: - return serial_get_write_free(port); - case DEVCTL_SET_BAUDRATE: - return serial_set_baudrate(port, (int32_t)extra_arg); - default: - errno = EINVAL; - return PROS_ERR; - } + dev_file_arg_t* file_arg = (dev_file_arg_t*)arg; + uint32_t port = file_arg->port; + switch (cmd) { + case DEVCTL_FIONREAD: + return serial_get_read_avail(port); + case DEVCTL_FIONWRITE: + return serial_get_write_free(port); + case DEVCTL_SET_BAUDRATE: + return serial_set_baudrate(port, (int32_t)extra_arg); + default: + errno = EINVAL; + return PROS_ERR; + } } /******************************************************************************/ /** Driver description **/ /******************************************************************************/ -const struct fs_driver _dev_driver = {.close_r = dev_close_r, - .fstat_r = dev_fstat_r, - .isatty_r = dev_isatty_r, - .lseek_r = dev_lseek_r, - .read_r = dev_read_r, - .write_r = dev_write_r, - .ctl = dev_ctl}; +const struct fs_driver _dev_driver = { + .close_r = dev_close_r, + .fstat_r = dev_fstat_r, + .isatty_r = dev_isatty_r, + .lseek_r = dev_lseek_r, + .read_r = dev_read_r, + .write_r = dev_write_r, + .ctl = dev_ctl +}; const struct fs_driver* const dev_driver = &_dev_driver; int dev_open_r(struct _reent* r, const char* path, int flags, int mode) { - if (*path == '\0') { - return STDOUT_FILENO; - } - - if (*path == '/') { - path++; - } - - // check length of path - it MUST be at most 2 characters - size_t i; - for (i = 0; i < 3; i++) { - if (path[i] == '\0') { - break; - } - } - int32_t port; - if (path[i] != '\0') { // i will the length of the path or the third character - r->_errno = ENAMETOOLONG; - return -1; - } - if (i == 2) { - // Port number is two characters - port = 10 * (path[0] - ASCII_ZERO) + (path[1] - ASCII_ZERO); - } else { - port = path[0] - ASCII_ZERO; - } - serial_enable(port); - - dev_file_arg_t* arg = (dev_file_arg_t*)kmalloc(sizeof(dev_file_arg_t)); - arg->port = port; - arg->flags = flags; - return vfs_add_entry_r(r, dev_driver, arg); + if (*path == '\0') { + return STDOUT_FILENO; + } + + if (*path == '/') { + path++; + } + + // check length of path - it MUST be at most 2 characters + size_t i; + for (i = 0; i < 3; i++) { + if (path[i] == '\0') { + break; + } + } + int32_t port; + if (path[i] != '\0') { // i will the length of the path or the third character + r->_errno = ENAMETOOLONG; + return -1; + } + if (i == 2) { + // Port number is two characters + port = 10 * (path[0] - ASCII_ZERO) + (path[1] - ASCII_ZERO); + } else { + port = path[0] - ASCII_ZERO; + } + serial_enable(port); + + dev_file_arg_t* arg = (dev_file_arg_t*)kmalloc(sizeof(dev_file_arg_t)); + arg->port = port; + arg->flags = flags; + return vfs_add_entry_r(r, dev_driver, arg); } From fae81145687cb7a64148de45bc54e9bef2841f24 Mon Sep 17 00:00:00 2001 From: Exortions Date: Fri, 28 Feb 2025 11:25:45 -0800 Subject: [PATCH 09/11] =?UTF-8?q?=F0=9F=90=9B=20Initialize=20registry=20in?= =?UTF-8?q?=20vdml?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/devices/vdml.cpp | 19 +++++++++++-------- 1 file changed, 11 insertions(+), 8 deletions(-) diff --git a/src/devices/vdml.cpp b/src/devices/vdml.cpp index da2dcd7f..fde5783d 100644 --- a/src/devices/vdml.cpp +++ b/src/devices/vdml.cpp @@ -5,26 +5,29 @@ #include #include +using namespace zest::vdml; + // initialize the device mutex array -std::array zest::vdml::device_mutexes; +std::array device_mutexes; -std::mutex zest::vdml::create_mutex() { +std::mutex create_mutex() { // for now just a stub, as don't know what type of mutex will be implemented yet return std::mutex(); } -void zest::vdml::initialize_vdml() { +void initialize_vdml() { // initialize the device (port) mutexes - for (int i = 0; i < zest::vdml::MAX_DEVICE_PORTS; i++) - zest::vdml::device_mutexes[i] = create_mutex(); + for (int i = 0; i < MAX_DEVICE_PORTS; i++) + device_mutexes[i] = create_mutex(); - // initialize ADI port mutexes + // initialize register + initialize_registry(); } -std::mutex& zest::vdml::smart_port_mutex(int8_t port) { +std::mutex& smart_port_mutex(int8_t port) { port = abs(port); // prevent negative port - return zest::vdml::device_mutexes[port]; + return device_mutexes[port]; } bool is_valid_port(uint8_t port) { From 67f9ab76c9202452de39dc000520c957d1978c4e Mon Sep 17 00:00:00 2001 From: Exortions Date: Fri, 28 Feb 2025 13:20:36 -0800 Subject: [PATCH 10/11] =?UTF-8?q?=F0=9F=90=9B=20Use=20PROS=20mutex?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- include/vdml/vdml.hpp | 8 ++++---- src/devices/vdml.cpp | 25 ++++++++----------------- 2 files changed, 12 insertions(+), 21 deletions(-) diff --git a/include/vdml/vdml.hpp b/include/vdml/vdml.hpp index b5d2e511..ef71a7d5 100644 --- a/include/vdml/vdml.hpp +++ b/include/vdml/vdml.hpp @@ -2,18 +2,18 @@ #define VDML_HPP #include "pros/device.h" +#include "pros/rtos.hpp" #include -#include namespace zest { namespace vdml { constexpr uint8_t MAX_DEVICE_PORTS = 32; -extern std::array device_mutexes; +static std::array device_mutexes; -std::mutex& smart_port_mutex(int port); -std::mutex create_mutex(); +pros::Mutex* smart_port_mutex(uint8_t port); +pros::Mutex* create_mutex(); void initialize_vdml(); diff --git a/src/devices/vdml.cpp b/src/devices/vdml.cpp index fde5783d..4f14ae4f 100644 --- a/src/devices/vdml.cpp +++ b/src/devices/vdml.cpp @@ -1,33 +1,24 @@ #include "vdml/vdml.hpp" +#include "pros/rtos.hpp" #include "vdml/registry.hpp" -#include -#include - -using namespace zest::vdml; - -// initialize the device mutex array -std::array device_mutexes; - -std::mutex create_mutex() { +pros::Mutex* create_mutex() { // for now just a stub, as don't know what type of mutex will be implemented yet - return std::mutex(); + return new pros::Mutex(); } void initialize_vdml() { // initialize the device (port) mutexes - for (int i = 0; i < MAX_DEVICE_PORTS; i++) - device_mutexes[i] = create_mutex(); + for (int i = 0; i < zest::vdml::MAX_DEVICE_PORTS; i++) + zest::vdml::device_mutexes[i] = zest::vdml::create_mutex(); // initialize register - initialize_registry(); + zest::vdml::initialize_registry(); } -std::mutex& smart_port_mutex(int8_t port) { - port = abs(port); // prevent negative port - - return device_mutexes[port]; +pros::Mutex* smart_port_mutex(uint8_t port) { + return zest::vdml::device_mutexes.at(port); } bool is_valid_port(uint8_t port) { From 6e07d28e039b079519d8b449f3c3bad6de42a1d2 Mon Sep 17 00:00:00 2001 From: Exortions Date: Fri, 28 Feb 2025 13:22:12 -0800 Subject: [PATCH 11/11] =?UTF-8?q?=F0=9F=90=9B=20Utilize=20static=5Fcast?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/devices/vdml_registry.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/devices/vdml_registry.cpp b/src/devices/vdml_registry.cpp index 94593860..ae0d9301 100644 --- a/src/devices/vdml_registry.cpp +++ b/src/devices/vdml_registry.cpp @@ -10,8 +10,8 @@ void initialize_registry() { // update registry types update_registry(); for (int i = 0; i < 22; i++) { - device_registry[i]->type = (DeviceType)registry_types[i]; - device_registry[i]->data = (int)vexDeviceGetByIndex(i); + device_registry[i]->type = static_cast(registry_types[i]); + device_registry[i]->data = (uint32_t)vexDeviceGetByIndex(i); } } @@ -51,7 +51,7 @@ bool validate_device(uint8_t port, DeviceType expected) { return false; DeviceType registered = device_registry[port]->type; - DeviceType actual = (DeviceType)registry_types[port]; + DeviceType actual = static_cast(registry_types[port]); // automatically register the port, if needed if (registered == DeviceType::NONE && actual != DeviceType::NONE) {