diff --git a/include/vdml/port.h b/include/vdml/port.h deleted file mode 100644 index b6f102d2..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 2f8b64a1..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/registry.hpp b/include/vdml/registry.hpp new file mode 100644 index 00000000..ae8357a1 --- /dev/null +++ b/include/vdml/registry.hpp @@ -0,0 +1,58 @@ +#ifndef VDML_REGISTRY_HPP +#define VDML_REGISTRY_HPP + +#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 +}; + +struct DeviceInfo { + DeviceType type; + uint32_t data; +}; + +static std::array, MAX_DEVICE_PORTS> device_registry; +static V5_DeviceType registry_types[MAX_DEVICE_PORTS]; + +void initialize_registry(); + +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 + +// 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.h b/include/vdml/vdml.h deleted file mode 100644 index c10006d5..00000000 --- a/include/vdml/vdml.h +++ /dev/null @@ -1,245 +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 "vdml/registry.h" - -#include -#include - -#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/include/vdml/vdml.hpp b/include/vdml/vdml.hpp new file mode 100644 index 00000000..ef71a7d5 --- /dev/null +++ b/include/vdml/vdml.hpp @@ -0,0 +1,56 @@ +#ifndef VDML_HPP +#define VDML_HPP + +#include "pros/device.h" +#include "pros/rtos.hpp" + +#include + +namespace zest { +namespace vdml { +constexpr uint8_t MAX_DEVICE_PORTS = 32; + +static std::array device_mutexes; + +pros::Mutex* smart_port_mutex(uint8_t port); +pros::Mutex* create_mutex(); + +void initialize_vdml(); + +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 diff --git a/src/devices/battery.c b/src/devices/battery.c index e718ce5e..a227967e 100644 --- a/src/devices/battery.c +++ b/src/devices/battery.c @@ -12,7 +12,7 @@ */ #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)) { diff --git a/src/devices/registry.c b/src/devices/registry.c index 8dff0557..d35fbe61 100644 --- a/src/devices/registry.c +++ b/src/devices/registry.c @@ -12,13 +12,13 @@ * file, You can obtain one at http://mozilla.org/MPL/2.0/. */ -#include "vdml/registry.h" +#include "vdml/registry.hpp" #include "kapi.h" #include "pros/apix.h" #include "pros/misc.h" #include "v5_api.h" -#include "vdml/vdml.h" +#include "vdml/vdml.hpp" #include #include @@ -149,4 +149,4 @@ int32_t registry_validate_binding(uint8_t port, v5_device_e_t expected_t) { errno = EADDRINUSE; return 2; } -} +} \ No newline at end of file diff --git a/src/devices/vdml.c b/src/devices/vdml.c deleted file mode 100644 index 083f15da..00000000 --- a/src/devices/vdml.c +++ /dev/null @@ -1,268 +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 "vdml/registry.h" - -#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 -} diff --git a/src/devices/vdml.cpp b/src/devices/vdml.cpp new file mode 100644 index 00000000..4f14ae4f --- /dev/null +++ b/src/devices/vdml.cpp @@ -0,0 +1,26 @@ +#include "vdml/vdml.hpp" + +#include "pros/rtos.hpp" +#include "vdml/registry.hpp" + +pros::Mutex* create_mutex() { + // for now just a stub, as don't know what type of mutex will be implemented yet + return new pros::Mutex(); +} + +void initialize_vdml() { + // initialize the device (port) mutexes + for (int i = 0; i < zest::vdml::MAX_DEVICE_PORTS; i++) + zest::vdml::device_mutexes[i] = zest::vdml::create_mutex(); + + // initialize register + zest::vdml::initialize_registry(); +} + +pros::Mutex* smart_port_mutex(uint8_t port) { + return zest::vdml::device_mutexes.at(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..ae0d9301 --- /dev/null +++ b/src/devices/vdml_registry.cpp @@ -0,0 +1,75 @@ +#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 = static_cast(registry_types[i]); + device_registry[i]->data = (uint32_t)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 = static_cast(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; + } +}