diff --git a/firmware/sylib.a b/firmware/sylib.a new file mode 100644 index 0000000..18e820e Binary files /dev/null and b/firmware/sylib.a differ diff --git a/include/api.h b/include/api.h index 718030d..5292beb 100644 --- a/include/api.h +++ b/include/api.h @@ -41,8 +41,8 @@ #define PROS_VERSION_MAJOR 3 #define PROS_VERSION_MINOR 7 -#define PROS_VERSION_PATCH 3 -#define PROS_VERSION_STRING "3.7.3" +#define PROS_VERSION_PATCH 2 +#define PROS_VERSION_STRING "3.7.2" #include "pros/adi.h" #include "pros/colors.h" diff --git a/include/main.h b/include/main.h index 57e23d5..87c1892 100644 --- a/include/main.h +++ b/include/main.h @@ -33,8 +33,10 @@ * For instance, you can do `4_mtr = 50` to set motor 4's target velocity to 50 */ #define PROS_USE_LITERALS -#include "pros/apix.h" + #include "api.h" +#include "pros/apix.h" +#include "sylib/sylib.hpp" /** * You should add more #includes here @@ -53,7 +55,8 @@ * namespace. */ // using namespace pros; - using namespace okapi::literals; +// using namespace pros::literals; +using namespace okapi::literals; // using namespace okapi; /** diff --git a/include/pros/motors.hpp b/include/pros/motors.hpp index da330a9..77abcc8 100644 --- a/include/pros/motors.hpp +++ b/include/pros/motors.hpp @@ -1020,29 +1020,6 @@ class Motor_Group { /** These functions let programmers configure the behavior of motor groups **/ /****************************************************************************/ - /** - * Indexes Motor in the Motor_Group in the same way as an array. - * - * This function uses the following values of errno when an error state is - * reached: - * ENXIO - Out of bounds on indexing the motor groups. - * - * \param i - * The index value in the motor group. - * - * \return the appropriate Motor reference or the erno if the operation - * failed - */ - pros::Motor& operator[](int i); - - - /** - * Indexes Motor in the Motor_Group in the same way as an array. - * - * \return the size of the vector containing motors - */ - std::int32_t size(); - /** * Sets the position for the motor in its encoder units. * diff --git a/include/sylib/addrled.hpp b/include/sylib/addrled.hpp new file mode 100644 index 0000000..667225e --- /dev/null +++ b/include/sylib/addrled.hpp @@ -0,0 +1,388 @@ +/** + * \file include/sylib/addrled.hpp + * + * \brief Contains prototypes for functions relating to individually + * addressable LED strips for the V5 + * + * 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 https://mozilla.org/MPL/2.0/. + */ + +#include +#include + +#include "env.hpp" +#include "sylib/sylib_apitypes.hpp" +#include "system.hpp" + +namespace sylib { + +/** + * \brief RGB Color + */ +typedef struct { + double r; + double g; + double b; +} rgb; + +/** + * \brief HSV Color + */ +typedef struct { + double h; + double s; + double v; +} hsv; + +/** + * \brief WS2812B Addressable LED Strip Controller + */ +class Addrled : private Device { + private: + static const std::vector off_buffer; + const std::uint8_t smart_port; + const std::uint8_t adi_port; + const std::uint8_t strip_length; + std::vector buffer; + std::vector template_buffer; + std::vector rotation_buffer; + std::vector buffer_saved; + static std::vector& getAllAddrleds(); + const V5_DeviceT device; + static void setAddrledUpdateCycles(int count); + sylib::SylibAddrledControlMode addrledControlMode; + double controlSpeed = 1; + double pulseSpeed = 1; + int cyclePixelsShifted = 0; + int end_pixel = -1; + bool pulseControlReversed = false; + bool cycleControlReversed = false; + int pixelsToMove = 0; + int pulseStartMovementTime = 0; + int cycleStartMovementTime = 0; + bool sendingPulse = false; + int controlPulseWidth = 1; + int redShift = 0; + int greenShift = 0; + int blueShift = 0; + int pulsePixelsShifted = 0; + + public: + /** + * \brief Creates an Addrled object for the given port and specifications. + * + * \param smart_port + * The V5 port number from 1-22 + * \param adi_port + * The ADI port number from 1-8 + * \param strip_length + * The number of pixels on the light strip + * \param colors + * Vector of hex codes to initialize the strip to + */ + Addrled(const std::uint8_t smart_port, const std::uint8_t adi_port, + const std::uint8_t strip_length, + const std::vector colors = std::vector()); + + ~Addrled(); + + /** + * \brief A flag that when set to false turns off all lights on all addrled objects + **/ + static bool addrled_enabled; + + /** + * \brief Sets every pixel to a single color + * + * \param color + * Hex color code + */ + void set_all(std::uint32_t color); + + /** + * \brief Sets a specific pixel to a specific color + * + * \param color + * Hex color code + * \param index + * Zero-indexed pixel number + */ + void set_pixel(std::uint32_t color, std::uint8_t index); + + /** + * \brief Sets the entire light strip to the provided vector of hex codes + * + * \param colors + * A buffer containing hex codes + */ + void set_buffer(std::vector colors); + + /** + * \brief Addrled update loop + * + * Updates the light strip to the most recently set values + * + * Users do not need to call this function, it is handled + * by the sylib daemon. Don't use this. It won't help. + */ + void update() override; + + /** + * \brief Sets the light control mode to OFF without changing the saved buffer + */ + void turn_off(); + + /** + * \brief Sets the light control mode to MANUAL without changing the saved buffer + */ + void turn_on(); + + /** + * \brief Resets the buffer + * + * Turns off the lights + */ + void clear(); + + /** + * \brief Sends a single pulse of color down the strip + * + * \param color + * Hex code for the desired pulse color + * + * \param pulse_width + * The number of pixels wide the pulse should be + * + * \param speed + * A value from 1-100 describing the speed of the pulse. 1 is slow, 100 is fast + * + * \param start_pos + * How many pixels down the strip to start + * + * \param reverse + * A flag that when true reverses the direction of the pulse + * + * \param end_pixel + * The pixel number to end the pulse on. If this unset it defaults to the end of the + * strip + */ + void pulse(std::uint32_t color, int pulse_width, int speed = 1, int start_pos = 0, + bool reverse = false, int end_pixel = -1); + + /** + * \brief Sets the strip to a static color gradient + * + * Does not modify the saved buffer, can be used over other control modes + * + * \param start_color + * Hex code for one endpoint of the gradient + * + * \param end_color + * Hex code for the other endpoint of the gradient + * + * \param fade_width + * A value describing how many pixels long to make the transition between colors. + * + * \param reverse + * How many pixels down the strip to start + * + * \param reverse + * A flag that when true reverses the direction of the gradient + * + * \param hsv_mode + * A flag that enables the gradient to work in HSV space rather than RGB + */ + void gradient(std::uint32_t start_color, std::uint32_t end_color, int fade_width = 0, + int start_pos = 0, bool reverse = false, bool hsv_mode = false); + + /** + * \brief Automatically rotates the colors of a provided buffer through the light strip + * periodically + * + * Sets the control mode to CYCLE + * + * \param color + * Buffer to rotate, use * to use the current saved buffer + * + * \param speed + * A value from 1-100 describing the speed of the pulse. 1 is slow, 100 is fast + * + * \param start_pos + * How many pixels down the strip to start + * + * \param reverse + * A flag that when true reverses the direction of the rotation + */ + void cycle(std::vector pattern, int speed = 1, int start_pos = 0, + bool reverse = false); + + /** + * \brief Rotates pixel colors up or down the strip + * + * \param pixels + * The number of pixels to augment the buffer by + * + * \param reverse + * A flag that when true reverses the direction of the rotation + */ + void rotate(int pixels = 0, bool reverse = false); + + /** + * \brief Adds the provided red, green, and blue values to the colors sent to the lights. + * + * Does not modify the saved buffer, can be used over other control modes + * + * \param red + * A number from -255 to 255 describing how much red to add + * + * \param green + * A number from -255 to 255 describing how much green to add + * + * \param blue + * A number from -255 to 255 describing how much blue to add + */ + void color_shift(int red, int green, int blue); + + /** + * \brief Gets smart port number used by the ADI expander for the light strip + * + * \return Value from 1-22 describing a smart port. 22 is the built-in three wire ports. + */ + int get_smart_port(); + + /** + * \brief Gets adi port number used by the light strip + * + * \return Value from 1-8 describing an adi port. + */ + int get_adi_port(); + + /** + * \brief Operator overload to retrieve a pixel color + * + * \param index + * Zero-indexed pixel number + * + * \return Hex code describing pixel color + */ + uint32_t& operator[](std::uint32_t index); + + /** + * \brief Operator overload to retrieve the current saved buffer + * + * \return Vector of hex codes describing pixel colors + */ + std::vector& operator*(); + + /** + * \brief Gets which control mode the light is using + * + * \return Control mode + */ + sylib::SylibAddrledControlMode getControlMode(); + + /** + * \brief Finds a color between two provided color values + * + * Uses the HSV color space + * + * \param start_color + * The color at the start of the gradient + * + * \param end_color + * The color at the end of the gradient + * + * \param step + * The number of steps of size (1/fade_width) from the start value towards the end value + * + * \param fade_width + * The number of pixels between the two endpoints + * + * \return Hex code describing pixel color + */ + static std::uint32_t interpolate_hsv(std::uint32_t start_color, std::uint32_t end_color, + int step, int fade_width); + + /** + * \brief Finds a color between two provided color values + * + * Uses the RGB color space + * + * \param start_color + * The color at the start of the gradient + * + * \param end_color + * The color at the end of the gradient + * + * \param step + * The number of steps of size (1/fade_width) from the start value towards the end value + * + * \param fade_width + * The number of pixels between the two endpoints + * + * \return Hex code describing pixel color + */ + static std::uint32_t interpolate_rgb(std::uint32_t start_color, std::uint32_t end_color, + int step, int fade_width); + + /** + * \brief Converts an HSV color to an RGB hex code + * + * \param in + * HSV color to translate + * + * \return Hex code describing an RGB color + */ + static std::uint32_t hsv_to_rgb(hsv in); + + /** + * \brief Converts RGB values to an HSV color + * + * \param r + * Red amount + * + * \param g + * Green amount + * + * \param b + * Blue amount + * + * \return HSV color + */ + static hsv rgb_to_hsv(std::uint32_t in); + + /** + * \brief Converts RGB values to an RGB hex code + * + * \param r + * Red amount + * + * \param g + * Green amount + * + * \param b + * Blue amount + * + * \return Hex code describing an RGB color + */ + static std::uint32_t rgb_to_hex(int r, int g, int b); + + /** + * \brief Converts an RGB hex code to RGB values + * + * \param r + * Red amount + * + * \param g + * Green amount + * + * \param b + * Blue amount + * + * \return RGB color + */ + static rgb hex_to_rgb(std::uint32_t color); +}; +} // namespace sylib \ No newline at end of file diff --git a/include/sylib/env.hpp b/include/sylib/env.hpp new file mode 100644 index 0000000..59371da --- /dev/null +++ b/include/sylib/env.hpp @@ -0,0 +1,49 @@ +/** + * \file sylib/env.hpp + * + * \brief Includes needed PROS or VEXcode header files, depending on which enviroment is being used + * + * 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 https://mozilla.org/MPL/2.0/. + */ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// #define SYLIB_SRC_PRESENT +#define SYLIB_ENV_PROS + +// #define SYLIB_ENV_VEXCODE + +#ifdef SYLIB_ENV_PROS +#ifdef SYLIB_SRC_PRESENT +#include "pros.h" +#else +#include "pros_includes.h" +#endif +#elif defined(SYLIB_ENV_VEXCODE) +#include "vex.h" +#endif + +extern "C" { +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); +} \ No newline at end of file diff --git a/include/sylib/math.hpp b/include/sylib/math.hpp new file mode 100644 index 0000000..c15b10e --- /dev/null +++ b/include/sylib/math.hpp @@ -0,0 +1,836 @@ +/** + * \file include/sylib/math.hpp + * + * \brief Contains prototypes for various controllers and math utilities + * + * 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 https://mozilla.org/MPL/2.0/. + */ + +#include "env.hpp" + +namespace sylib { +using kv_fn_t = std::function; + +/** + * \brief Exponential Moving Average Filter + */ +class EMAFilter { + private: + double inputkA; + double ema; + + public: + /** + * \brief Creates an Exponential Moving Average filter + */ + EMAFilter(); + + /** + * \brief Filters an input value + * + * \param rawValue + * The raw input value to filter + * \param kA + * How much the filter will condider previous information. A value of 1 + * will make the input the full value of the output, a value of 0 will + * make the input have no effect on the output. + * + * \return Filter output + */ + double filter(double rawValue, double kA); + + /** + * \brief Gets the most recently used kA value + * + * \return kA value + */ + double getkA() const; + + /** + * \brief Gets the current filter output + * + * \return Filter output + */ + double getCurrentEMA() const; +}; + +/** + * \brief Simple Moving Average Filter + */ +class SMAFilter { + private: + std::queue rawInputValues; + std::uint16_t sampleSize; + double meanValue; + double rawVelocityTotal; + + public: + /** + * \brief Creates a Simple Moving Average filter + * + * \param sampleSize + * The number of previous values to average. + * A higher number will result in a more stable value, + * but with more lag. + */ + SMAFilter(int sampleSize); + + /** + * \brief Filters an input value + * + * \param rawValue + * The raw input value to filter + * + * \return Filter output + */ + double filter(double rawValue); + + /** + * \brief Gets the current length of the queue of previous values + * + * \return Queue length + */ + int getQueueLength() const; + + /** + * \brief Gets the maximum length of the queue of previous values + * + * Equal to the sample size specifiied in the constructor + * + * \return Max sample size + */ + int getQueueMaxLength() const; + + /** + * \brief Gets the current sum of queue values + * + * \return Sum of values + */ + double getCurrentTotal() const; + + /** + * \brief Gets the current filter output without providing a new input value + * + * \return Filter output + */ + double getCurrentValue() const; +}; + +/** + * \brief Median Filter + */ +class MedianFilter { + private: + std::deque rawInputValues; + int sampleSize; + int queueLength; + double medianValue; + int meanSizeEven; + int meanSizeOdd; + + public: + /** + * \brief Creates a Median filter + * + * \param sampleSize + * The number of previous values to consider. + * A higher number will result in a more stable value, + * but with more lag. + * + * \param meanSizeEven + * The number of median values to average when the sample size is even + * + * \param meanSizeOdd + * The number of median values to average when the sample size is odd + */ + MedianFilter(int sampleSize, int meanSizeEven, int meanSizeOdd); + + /** + * \brief Filters an input value + * + * \param rawValue + * The raw input value to filter + * + * \return Filter output + */ + double filter(double rawValue); + + /** + * \brief ets the current length of the queue of previous values + * + * \return Queue length + */ + int getQueueLength() const; + + /** + * \brief Gets the current sum of queue values + * + * \return Sum of values + */ + int getQueueMaxLength() const; + + /** + * \brief Gets the number of median values to average when the sample size is even + * + * \return Center size + */ + int getEvenCenterSize() const; + + /** + * \brief Gets the number of median values to average when the sample size is odd + * + * \return Center size + */ + int getOddCenterSize() const; + + /** + * \brief Gets the current filter output without providing a new input value + * + * \return Filter output + */ + double getCurrentValue() const; +}; + +/** + * \brief Extrema Filter + */ +class RangeExtremaFilter { + private: + std::deque rawInputValues; + double maxValue; + int queueLength; + int sampleSize; + + public: + /** + * \brief Creates an Extrema filter that returns the largest absolute value of + * recent inputs + * + * \param sampleSize + * The number of previous values to consider. + * A higher number will result in a more stable value, + * but with more lag. + */ + RangeExtremaFilter(int sampleSize); + + /** + * \brief Filters an input value + * + * \param rawValue + * The raw input value to filter + * + * \return Filter output + */ + double filter(double rawValue); + + /** + * \brief Gets the current length of the queue of previous values + * + * \return Queue length + */ + int getQueueLength() const; + + /** + * \brief Gets the number of median values to average when the sample size is even + * + * \return Center size + */ + int getQueueMaxLength() const; + + /** + * \brief Gets the current filter output without providing a new input value + * + * \return Filter output + */ + double getCurrentValue() const; +}; + +/** + * \brief Generic Derivative Solver + */ +class SympleDerivativeSolver { + private: + double currentInputFunctionValue; + double previousInputFunctionValue; + double deltaInputFunctionValue; + uint32_t currentTime; + uint32_t previousTime; + uint32_t dT; + double derivativeFunctionValue; + + public: + /** + * \brief Creates a generic derivative solver that outputs the dX/dT, + * where X is the input value provided, and T is the time in + * milliseconds since the last input. Keeps track of its own timer. + * + */ + SympleDerivativeSolver(); + + /** + * \brief Calculates the slope between the last input value and the current input value + * + * \param input + * The raw input value + * + * \return Derivative value + */ + double solveDerivative(double input); + + /** + * \brief Gets the current stored output value without adding any new input + * + * \return Derivative value + */ + double getCurrentDerivative() const; + + /** + * \brief Gets the most recent input value + * + * \return Input value + */ + double getCurrentInputValue() const; +}; + +/** + * \brief Feedforward Controller + */ +class VoltageEstimation { + private: + kv_fn_t kV; + double voltageEstimate; + double motorGearing; + + public: + /** + * \brief Creates a simple feedforward controller for a V5 motor + * + * \param kV + * A lambda function that takes double as an input for target RPM + * and returns a voltage constant to use for the estimation + * + * \param motorGearing + * The output speed of the motor, in RPM, if the motor internally + * is spinning at 3600 RPM. 200 is default for a green motor cartridge + */ + VoltageEstimation(kv_fn_t kV, double motorGearing = 200); + + /** + * \brief Outputs an estimated voltage value for acheiving the desired RPM based + * on the kV function supplied in the constructor + * + * \param rpm + * The target velocity of the motor, in RPM + */ + double estimate(double rpm); + + /** + * \brief Gets the current kV function + * + * \return kV function + */ + kv_fn_t getKv() const; + + /** + * \brief Gets the set motor gearing for the estimator + * + * \return Output RPM of the motor at 100% velocity + */ + double getMotorGearing() const; + + /** + * \brief Sets the kV function + * + * \param value + * A lambda function that takes double as an input for target RPM + * and returns a voltage constant to use for the estimation + */ + void setkV(kv_fn_t value); + + /** + * \brief Gets the current stored output value without adding any new input + * + * \return Voltage estimate + */ + double getOutput() const; +}; + +/** + * \brief P Controller + */ +class ProportionalController { + private: + + double kP; + double motorGearing; + std::shared_ptr error; + double maxRange; + bool maxRangeEnabled; + double kP2; + double proportional; + + + + public: + /** + * \brief Creates a P controller for a V5 motor + * + * \param kP + * Proportional gain + * + * \param error + * A pointer to a double containing the error value to use for calculations + * + * \param motorGearing + * The output speed of the motor, in RPM, if the motor internally + * is spinning at 3600 RPM. 200 is default for a green motor cartridge + * + * \param maxRangeEnabled + * A flag that when true shifts the P controller to using kP2 as the kP constant when + * error is less than + * + * \param kP2 + * The secondary kP value that is used when maxRangeEnabled is on and the error is within + * maxRange + * + * \param maxRange + * Error range from the target value that is used for switching between kP and kP2 + */ + ProportionalController(double kP, std::shared_ptr error, double motorGearing = 200, + bool maxRangeEnabled = false, double kP2 = 0, double maxRange = 0); + + /** + * \brief Calculates controller output based on the current error value + * + * \return Controller output + */ + double update(); + + /** + * \brief Gets the kP constant + * + * \return kP value + */ + double getkP() const; + + /** + * \brief Gets the current stored output value without calculating a new value + * + * \return Controller output + */ + double getOutput() const; + + /** + * \brief Sets the kP constant + * + * \param gain + * Proportional gain + */ + void setkP(double gain); + + /** + * \brief Operator overload for getting the current filter ouput + * + * \return Controller output + */ + double operator*(); + + /** + * \brief Sets whether the maxRangeEnabled flag is on or off + * + * \param enabled + * Flag value + */ + void setMaxRangeEnabled(bool enabled); + + /** + * \brief Sets the error range from the target value that is used for switching between kP and + * kP2 + * + * \param range + * Max error value + */ + void setMaxRange(double range); + + /** + * \brief Sets the secondary kP constant + * + * \param gain + * Proportional gain + */ + void setkP2(double gain); +}; + +/** + * \brief I Controller + */ +class IntegralController { + private: + + double kI; + double motorGearing; + std::shared_ptr error; + bool antiWindupEnabled; + double antiWindupRange; + double integral; + uint32_t currentTime; + uint32_t previousTime; + uint32_t dT; + + + public: + /** + * \brief Creates an I controller for a V5 motor + * + * \param kI + * Integral gain + * + * \param error + * A pointer to a double containing the error value to use for calculations + * + * \param motorGearing + * The output speed of the motor, in RPM, if the motor internally + * is spinning at 3600 RPM. 200 is default for a green motor cartridge + * + * \param antiWindupEnabled + * A flag that when true enables anti-windup, setting the output value to 0 when + * error is more than + * + * \param antiWindupRange + * Error range from the target value that is used for anti-windup + */ + IntegralController(double kI, std::shared_ptr error, double motorGearing = 200, + bool antiWindupEnabled = false, double antiWindupRange = 0); + + /** + * \brief Calculates controller output based on the current error value + * + * \return Controller output + */ + double update(); + + /** + * \brief Gets the kI constant + * + * \return kI value + */ + double getkI() const; + + /** + * \brief Resets the integral value to 0 + */ + void resetValue(); + + /** + * \brief Gets the current stored output value without calculating a new value + * + * \return Controller output + */ + double getOutput() const; + + /** + * \brief Gets the current time the controller is using + * + * \return Time in milliseconds + */ + double getCurrentTime() const; + + /** + * \brief Gets the most recent time difference between updates + * + * \return Time in milliseconds + */ + uint32_t getdT() const; + + /** + * \brief Sets the kI constant + * + * \param gain + * Integral gain + */ + void setkI(double gain); + + /** + * \brief Operator overload for getting the current filter ouput + * + * \return Controller output + */ + double operator*(); + + /** + * \brief Sets whether the antiWindupEnabled flag is on or off + * + * \param enabled + * Flag value + */ + void setAntiWindupEnabled(bool enabled); + + /** + * \brief Sets the error range from the target value that is used for anti-windup + * + * \param range + * Max error value + */ + void setAntiWindupRange(double range); +}; + +/** + * \brief D Controller + */ +class DerivativeController { + private: + double kD; + double motorGearing; + std::shared_ptr error; + double derivative; + double currentInput; + double previousInput; + uint32_t currentTime; + uint32_t previousTime; + uint32_t dT; + + public: + /** + * \brief Creates a D controller for a V5 motor + * + * \param kD + * Derivative gain + * + * \param error + * A pointer to a double containing the error value to use for calculations + * + * \param motorGearing + * The output speed of the motor, in RPM, if the motor internally + * is spinning at 3600 RPM. 200 is default for a green motor cartridge + */ + DerivativeController(double kD, std::shared_ptr error, double motorGearing = 200); + + /** + * \brief Calculates controller output based on the current error value + * + * \return Controller output + */ + double update(); + + /** + * \brief Gets the kD constant + * + * \return kD value + */ + double getkD() const; + + /** + * \brief Gets the current stored output value without calculating a new value + * + * \return Controller output + */ + double getOutput() const; + + /** + * \brief Gets the current time the controller is using + * + * \return Time in milliseconds + */ + double getCurrentTime() const; + + /** + * \brief Gets the most recent time difference between updates + * + * \return Time in milliseconds + */ + uint32_t getdT() const; + + /** + * \brief Sets the kD constant + * + * \param gain + * Derivative gain + */ + void setkD(double gain); + + /** + * \brief Operator overload for getting the current filter ouput + * + * \return Controller output + */ + double operator*(); +}; + +/** + * \brief TBH Controller + */ +class TakeBackHalfController { + private: + std::shared_ptr error; + double kH; + double output; + double tbh; + double previousError; + uint32_t currentTime; + uint32_t previousTime; + uint32_t dT; + + public: + /** + * \brief Creates an Take Back Half controller for a V5 motor + * + * \param kH + * TBH gain + * + * \param error + * A pointer to a double containing the error value to use for calculations + */ + TakeBackHalfController(double kH, std::shared_ptr error); + + /** + * \brief Calculates controller output based on the current error value + * + * \return Controller output + */ + double update(); + + /** + * \brief Gets the current stored output value without calculating a new value + * + * \return Controller output + */ + double getOutput() const; + + /** + * \brief Gets the kH constant + * + * \return kH value + */ + double getkH() const; + + /** + * \brief Gets the current stored TBH value + * + * \return TBH value + */ + double getTBH() const; + + /** + * \brief Sets the kH constant + * + * \param gain + * TBH gain + */ + void setkH(double gain); + + /** + * \brief Gets the current time the controller is using + * + * \return Time in milliseconds + */ + double getCurrentTime() const; + + /** + * \brief Operator overload for getting the current filter ouput + * + * \return Controller output + */ + double operator*() const; +}; + +/** + * \brief Custom Velocity Controller + * + * To disable a particular controller, set its gain to 0. + */ +struct SpeedControllerInfo { + /** + * \brief Lambda function to use for feedforward kV calculation + */ + kv_fn_t kV = [](double rpm) { return 0; }; + + /** + * \brief P controller gain + */ + double kP = 0; + + /** + * \brief I controller gain + */ + double kI = 0; + + /** + * \brief D controller gain + */ + double kD = 0; + + /** + * \brief TBH controller gain + */ + double kH = 0; + + /** + * \brief Whether or not to enable anti-windup on the I controller + */ + bool antiWindupEnabled = false; + + /** + * \brief Range to use for the anti-windup on the I controller, if enabled + */ + double antiWindupRange = 0; + + /** + * \brief Whether or not to change the P controller gain within a certain target range + */ + bool pRangeEnabled = false; + + /** + * \brief Range to use for changing P controller gain, if enabled + */ + double pRange = 0; + + /** + * \brief Secondary kP value that the P controller uses when inside the target range, if enabled + */ + double kP2 = 0; + + /** + * \brief Positive range from target above which the motor applies max voltage outside of + */ + double maxVoltageRange = 0; + + /** + * \brief Whether or not to enable coast-down to more quickly move to a lower velocity. + */ + bool coastDownEnabled = false; + + /** + * \brief Negative range from target below which the applied voltage is multiplied by the + * coast-down modifier + */ + double coastDownRange = 0; + + /** + * \brief Constant to multiple the applied voltage by if error is below the coast-down range + */ + double coastDownModifier = 1; + + SpeedControllerInfo( + kv_fn_t kV = [](double rpm) { return 0; }, double kP = 0, double kI = 0, double kD = 0, + double kH = 0, bool antiWindupEnabled = false, double antiWindupRange = 0, + bool pRangeEnabled = false, double pRange = 0, double kP2 = 0, double maxVoltageRange = 0, + bool coastDownEnabled = false, double coastDownRange = 0, double coastDownModifier = 1) + : kV(kV), + kP(kP), + kI(kI), + kD(kD), + kH(kH), + antiWindupEnabled(antiWindupEnabled), + antiWindupRange(antiWindupRange), + pRangeEnabled(pRangeEnabled), + pRange(pRange), + kP2(kP2), + maxVoltageRange(maxVoltageRange), + coastDownEnabled(coastDownEnabled), + coastDownRange(coastDownRange), + coastDownModifier(coastDownModifier) {} +}; + +} // namespace sylib \ No newline at end of file diff --git a/include/sylib/motor.hpp b/include/sylib/motor.hpp new file mode 100644 index 0000000..444ac99 --- /dev/null +++ b/include/sylib/motor.hpp @@ -0,0 +1,672 @@ +/** + * \file include/sylib/motor.hpp + * + * \brief Contains prototypes for functions relating to smart motor + * control and telemetry + * + * 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 https://mozilla.org/MPL/2.0/. + */ + +#pragma once +#include "env.hpp" +#include "sylib.hpp" +#include "sylib_apitypes.hpp" +#include "system.hpp" + +namespace sylib { + +/** + * \brief Accurate Motor Velocity Estimator + */ +class SylviesPogVelocityEstimator { + private: + sylib::SMAFilter smaFilterVelocity; + sylib::SMAFilter smaFilterAccel; + sylib::SMAFilter smaFilterJerk; + sylib::SMAFilter smaFilterSnap; + sylib::MedianFilter medianFilter; + sylib::EMAFilter emaFilter; + double motorGearing; + sylib::SympleDerivativeSolver preFilterAccelSolver; + sylib::SympleDerivativeSolver preFilterJerkSolver; + sylib::SympleDerivativeSolver preFilterSnapSolver; + sylib::SympleDerivativeSolver outputAccelSolver; + sylib::SympleDerivativeSolver outputJerkSolver; + sylib::SympleDerivativeSolver outputSnapSolver; + sylib::RangeExtremaFilter maxFilterAccel; + + + uint32_t previousInternalMotorClock; + double oldMotorTicks; + double dN; + double dT; + double rawVelocity; + double motorVoltageTarget; + double emaFilteredVelocity; + double smaFilteredVelocity; + double smaFilteredAccel; + double medianFilteredVelocity; + double outputVelocity; + double preFilterAcceleration; + double preFilterJerk; + double preFilterSnap; + double outputAcceleration; + double outputJerk; + double outputSnap; + double accelCalculatedkA; + double maxFilteredAccel; + double smaFilteredJerk; + double smaFilteredSnap; + + + + + + + + + public: + /** + * \brief Creates a velocity estimator for a smart motor + * + * \param motorGearing + * The output speed of the motor, in RPM, if the motor internally + * is spinning at 3600 RPM. 200 is default for a green motor cartridge + */ + SylviesPogVelocityEstimator(double motorGearing = 200); + + /** + * \brief Calculates motor velocity + * + * \param currentMotorTicks + * The current number of raw encoder ticks the motor has recorded + * + * \return Velocity in RPM + */ + double getVelocity(double currentMotorTicks, std::uint32_t currentInternalMotorClock); + + /** + * \brief Gets the most recent calculated motor velocity + * + * \return Velocity in RPM + */ + double getVelocityNoCalculations() const; + + /** + * \brief Gets the raw motor velocity + * + * This value is not filtered, and will be of limitied use. + * It is purely difference in ticks divided by difference in time. + * + * \return Velocity in RPM out of 3600 + */ + double getRawVelocity() const; + + /** + * \brief Gets the most recent user-provided motor position + * + * \return Ticks + */ + double getRawPosition() const; + + /** + * \brief Gets the motor velocity put through a 3-tap simple moving average + * + * This value is partially filtered, and will be of limitied use. + * + * \return Velocity in RPM out of 3600 + */ + double getSmaFilteredVelocity() const; + + /** + * \brief Gets the motor velocity put through a an exponential moving average filter + * + * This value is the same as the estimator's output velocity, but without the motor gearset + * translation. + * + * \return Velocity in RPM out of 3600 + */ + double getEmaFilteredVelocity() const; + + /** + * \brief Gets the motor velocity put through a median filter + * + * This value is partially filtered, and will be of limitied use. + * + * \return Velocity in RPM out of 3600 + */ + double getMedianFilteredVelocity() const; + + /** + * \brief Gets the motor acceleration + * + * This value is a second derivative of any actual measurement. + * It will not be very accurate. + * + * \return RPM per millisecond + */ + double getAcceleration() const; + + /** + * \brief Gets the raw motor acceleration based on a partially + * filtered velocity calculation + * + * This value is a second derivative of any actual measurement. + * It will not be very accurate. + * + * \return RPM per millisecond + */ + double getPreFilterAcceleration() const; + + /** + * \brief Gets the motor jerk + * + * This value is a third derivative of any actual measurement. + * It will not be very accurate. + * + * \return RPM per millisecond per millisecond + */ + double getJerk() const; + + /** + * \brief Gets the motor snap + * + * This value is a fourth derivative of any actual measurement. + * It will not be very accurate. + * + * \return RPM per millisecond per millisecond per millisecond + */ + double getSnap() const; + + /** + * \brief Gets the current value of the variable gain used for the + * velocity EMA filter + * + * \return kA value + */ + double getCalculatedkA() const; + + /** + * \brief Gets the absolute value of the largest or smallest acceleration + * measurement in the last 200ms + * + * This value is a second derivative of any actual measurement. + * It will not be very accurate. + * + * \return RPM per millisecond + */ + double getMaxFilteredAcceleration() const; +}; + +/** + * \brief V5 Smart Motor + */ +class Motor : private Device { + private: + const std::uint8_t smart_port; + const double gearing; + bool reversed; + const V5_DeviceT device; + void set_motor_controller(); + sylib::VoltageEstimation motorVoltageEstimator; + // Config settings + + + + sylib::SylviesPogVelocityEstimator motorVelocityTracker; + + sylib::ProportionalController motorPController; + sylib::IntegralController motorIController; + sylib::DerivativeController motorDController; + sylib::TakeBackHalfController motorTBHController; + sylib::SpeedControllerInfo speedController; + + // Updating values + uint32_t internalClock; + double velocity; + double velocity_error; + double raw_velocity; + double vexos_velocity; + double sma_velocity; + double acceleration; + double position; + double temperature; + double power; + double target_position; + double target_velocity; + std::int32_t current_draw; + std::int32_t direction; + std::int32_t efficiency; + std::int32_t faults; + std::int32_t flags; + double torque; + std::int32_t voltage; + std::int32_t zero_position_flag; + std::int32_t stopped; + std::int32_t over_temp; + std::int32_t over_current; + V5MotorBrakeMode brake_mode; + std::int32_t current_limit; + std::int32_t voltage_limit; + + // Target values + SylibMotorControlMode sylib_control_mode; + double position_target; + double velocity_target; + std::int32_t voltage_target; + + + /* + smart_port(smart_port), + gearing(gearing), + reversed(reverse), + device(vexDeviceGetByIndex(smart_port - 1)), + motorVoltageEstimator{speedController.kV, gearing}, + motorPController{speedController.kP, + std::shared_ptr(&velocity_error), + gearing, + true, + speedController.kP2, + speedController.pRange}, + motorIController{speedController.kI, std::shared_ptr(&velocity_error), gearing, true, + speedController.antiWindupRange}, + motorDController{speedController.kD, std::shared_ptr(&velocity_error), gearing}, + motorTBHController{speedController.kH, std::shared_ptr(&velocity_error)}, + speedController(speedController) { + */ + + public: + /** + * \brief Creates a smart motor object + * + * \param smart_port + * The 1-indexed smart port the motor uses + * + * \param gearing + * The output speed of the motor, in RPM, if the motor internally + * is spinning at 3600 RPM. 200 is default for a green motor cartridge + * + * \param reverse + * Optional motor reversed flag + * + * \param speedController + * Optional velocity controller used by set_velocity_custom_controller + */ + Motor(const uint8_t smart_port, const double gearing = 200, const bool reverse = false, + const SpeedControllerInfo speedController = SpeedControllerInfo()); + + // Background control stuff + + /** + * \brief Motor update loop + * + * Updates the motor to the most recently set values + * + * Users do not need to call this function, it is handled + * by the sylib daemon. Don't use this. It won't help. + */ + void update() override; + + // Motor control + + /** + * \brief Sets the absolute motor position target + * + * This uses the smart motor's own built-in controller + * + * \param new_position + * Absolute position to move to + * + * \param velocity_cap + * Target speed to perform the movement at + */ + void set_position_target_absolute(double new_position, std::int32_t velocity_cap = 200); + + /** + * \brief Sets a motor position target relative to its current position + * + * This uses the smart motor's own built-in controller + * + * \param new_position + * Position to move to + * + * \param velocity_cap + * Target speed to perform the movement at + */ + void set_position_target_relative(double new_position, std::int32_t velocity_cap = 200); + + /** + * \brief Sets the motor velocity target and changes control modes + * + * This uses a controller provided by the user + * + * \param new_velocity_target + * Motor velocity target + */ + void set_velocity_custom_controller(std::int16_t new_velocity_target); + + /** + * \brief Stops the motor + */ + void stop(); + + /** + * \brief Sets the voltage sent to the motor directly + * + * \param new_voltage_target + * Voltage in millivolts + */ + void set_voltage(std::int16_t new_voltage_target); + + /** + * \brief Sets the motor braking mode + * + * \param mode + * Brake mode to use + */ + void set_braking_mode(V5MotorBrakeMode mode); + + /** + * \brief Sets the motor amperage limit + * + * \param limit + * Limit in milliamps + */ + void set_amps_limit(std::int32_t limit); + + /** + * \brief Sets the motor reversed flag + * + * \param reverse + * Reverse flag + */ + void set_is_reversed(bool reverse); + + /** + * \brief Sets the motor voltage limit + * + * \param limit + * Limit in millivolts + */ + void set_volts_limit(std::int32_t limit); + + /** + * \brief Tares the motor encoder + */ + void tare_encoder(); + + /** + * \brief Sets the motor velocity target + * + * This uses the smart motor's own built-in controller + * + * \param new_velocity_target + * Motor velocity target + */ + void set_velocity(std::int32_t new_velocity_target); + + /** + * \brief Sets the custom speed controller used by set_velocity_custom_controller + * + * \param controller + * Velocity controller + */ + void updateSpeedController(sylib::SpeedControllerInfo controller); + + // Motor telemetry + + /** + * \brief Gets the current error between the motor's actual velocity and the velocity target + * + * Uses the sylib velocity estimator + * + * \return RPM + */ + double get_velocity_error() const; + + /** + * \brief Gets the current output of the user-provided P controller + * + * \return Millivolts + */ + std::int32_t get_p_voltage() const; + + /** + * \brief Gets the current output of the user-provided I controller + * + * \return Millivolts + */ + std::int32_t get_i_voltage() const; + + /** + * \brief Gets the current output of the user-provided D controller + * + * \return Millivolts + */ + std::int32_t get_d_voltage() const; + + /** + * \brief Gets the current output of the user-provided feedforward controller + * + * \return Millivolts + */ + std::int32_t get_estimator_voltage() const; + + /** + * \brief Gets the current output of the user-provided TBH controller + * + * \return Millivolts + */ + std::int32_t get_tbh_voltage() const; + + /** + * \brief Gets the current raw motor position + * + * \return Ticks + */ + double get_position() const; + + /** + * \brief Gets the current target motor position + * + * \return Ticks + */ + double get_position_target() const; + + /** + * \brief Gets the current motor velocity + * + * Uses the sylib velocity estimator + * + * \return RPM + */ + double get_velocity() const; + + /** + * \brief Gets the current motor velocity target + * + * \return RPM + */ + double get_velocity_target() const; + + /** + * \brief Gets the current motor velocity as reported by the motor itself + * + * This number is complete garbage and should not be used for anything. + * + * \return RPM + */ + double get_velocity_motor_reported() const; + + /** + * \brief Gets the unfiltered motor velocity + * + * Uses the sylib velocity estimator + * + * \return RPM + */ + double get_velocity_raw() const; + + /** + * \brief Gets the partially-filtered motor velocity + * + * Uses the sylib velocity estimator + * + * \return RPM + */ + double get_velocity_sma_filter_only() const; + + /** + * \brief Gets the motor acceleration + * + * Uses the sylib velocity estimator + * + * \return RPM per millisecond + */ + double get_acceleration() const; + + /** + * \brief Gets the motor temperature + * + * \return Degrees Celsius, rounded to the nearest 5 + */ + double get_temperature() const; + + /** + * \brief Gets the motor torque + * + * \return Newton-Metres + */ + double get_torque() const; + + /** + * \brief Gets the motor power consumption + * + * \return Watts + */ + double get_watts() const; + + /** + * \brief Gets the motor current draw + * + * \return Amps + */ + std::int32_t get_amps() const; + + /** + * \brief Gets the user-provided motor current limit + * + * \return Amps + */ + std::int32_t get_amps_limit() const; + + /** + * \brief Gets the voltage most recently sent to the motor, + * when using a sylib control mode + * + * \return Millivolts + */ + std::int32_t get_applied_voltage() const; + + /** + * \brief Gets the user-provided motor voltage limit + * + * \return Millivolts + */ + std::int32_t get_volts_limit() const; + + /** + * \brief Gets the motor-reported efficiency value in percent + * + * 100% efficiency means the motor is moving while drawing no power, + * and 0% efficiency means the motor is drawing power while not moving. + * + * \return Millivolts + */ + std::int32_t get_efficiency() const; + + /** + * \brief Gets the motor-reported faults + * + * \return Who knows ¯\\_(ツ)_/¯ + */ + std::int32_t get_faults() const; + + /** + * \brief Gets the motor-reported flags + * + * \return Who knows ¯\\_(ツ)_/¯ + */ + std::int32_t get_flags() const; + + /** + * \brief Gets the most recent time at which the motor has updated its tick count + * + * This value is based on the motor's internal clock, and drifts from the brain's system + * timer by approximately 1 part in 95. + * + * \return Milliseconds + */ + std::uint32_t get_device_timestamp() const; + + /** + * \brief Gets the current motor tick count and timestamp value + * + * Sets the value at the pointer provided to the motor timestamp in milliseconds + * + * \return Ticks + */ + std::int32_t get_position_and_timestamp(std::uint32_t* timestamp); + + /** + * \brief Gets the current motor brake mode + * + * \return V5MotorBrakeMode + */ + std::int32_t get_braking_mode() const; + + /** + * \brief Gets the user-set motor gearing + * + * \return RPM + */ + std::int32_t get_gearing() const; + + /** + * \brief Gets 1-indexed smart port used by the motor + * + * \return Smart Port Number + */ + std::int32_t get_smart_port() const; + + /** + * \brief Gets if the motor is stopped + * + * \return True if the most recent raw velocity measurement is 0, otherwise False + */ + bool is_stopped() const; + + /** + * \brief Gets if the motor is over current, as reported by the motor + * + * \return True or False + */ + bool is_over_current() const; + + /** + * \brief Gets if the motor is over temperature, as reported by the motor + * + * \return True or False + */ + bool is_over_temp() const; + + /** + * \brief Gets if the motor reversed flag is set + * + * \return True or False + */ + bool is_reversed() const; +}; +} // namespace sylib \ No newline at end of file diff --git a/include/sylib/pros_includes.h b/include/sylib/pros_includes.h new file mode 100644 index 0000000..088814e --- /dev/null +++ b/include/sylib/pros_includes.h @@ -0,0 +1,40 @@ +#pragma once +#include "api.h" + +#define V5_MAX_DEVICE_PORTS 32 + +typedef enum { + kDeviceTypeNoSensor = 0, + kDeviceTypeMotorSensor = 2, + kDeviceTypeLedSensor = 3, + kDeviceTypeAbsEncSensor = 4, + kDeviceTypeCrMotorSensor = 5, + kDeviceTypeImuSensor = 6, + kDeviceTypeRangeSensor = 7, + kDeviceTypeDistanceSensor = 7, + kDeviceTypeRadioSensor = 8, + kDeviceTypeTetherSensor = 9, + kDeviceTypeBrainSensor = 10, + kDeviceTypeVisionSensor = 11, + kDeviceTypeAdiSensor = 12, + kDeviceTypeRes1Sensor = 13, + kDeviceTypeRes2Sensor = 14, + kDeviceTypeRes3Sensor = 15, + kDeviceTypeOpticalSensor = 16, + kDeviceTypeMagnetSensor = 17, + kDeviceTypeGpsSensor = 20, + kDeviceTypeBumperSensor = 0x40, + kDeviceTypeGyroSensor = 0x46, + kDeviceTypeSonarSensor = 0x47, + kDeviceTypeGenericSensor = 128, + kDeviceTypeGenericSerial = 129, + kDeviceTypeUndefinedSensor = 255 +} V5_DeviceType; + +typedef struct _V5_Device* V5_DeviceT; + +typedef enum _V5_MotorBrakeMode { + kV5MotorBrakeModeCoast = 0, /// Motor will coast when stopped + kV5MotorBrakeModeBrake = 1, /// Motor will brake when stopped + kV5MotorBrakeModeHold = 2 /// Motor will hold position when stopped +} V5MotorBrakeMode; diff --git a/include/sylib/sylib.hpp b/include/sylib/sylib.hpp new file mode 100644 index 0000000..16f106a --- /dev/null +++ b/include/sylib/sylib.hpp @@ -0,0 +1,20 @@ +/** + * \file include/sylib/sylib.hpp + * + * \brief Includes all Sylib headers + * + * 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 https://mozilla.org/MPL/2.0/. + */ + +#pragma once + +#include "addrled.hpp" +#include "env.hpp" +#include "math.hpp" +#include "motor.hpp" +#include "sylib_apitypes.hpp" +#include "system.hpp" + +namespace sylib {} \ No newline at end of file diff --git a/include/sylib/sylib_apitypes.hpp b/include/sylib/sylib_apitypes.hpp new file mode 100644 index 0000000..05fa152 --- /dev/null +++ b/include/sylib/sylib_apitypes.hpp @@ -0,0 +1,31 @@ +/** + * \file include/sylib/sylib_apitypes.hpp + * + * \brief Contains enumerated types used by Sylib + * + * 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 https://mozilla.org/MPL/2.0/. + */ + +#pragma once + +namespace sylib { +typedef enum { + SylibMotorControlModeOFF = 0, /// Motor is off and in coast mode + SylibMotorControlModeBRAKE = 1, /// Motor is off and in brake mode + SylibMotorControlModeHOLD = 2, /// Motor is holding at current position + SylibMotorControlModeCUSTOM = 3, /// Motor is in velocity control mode + SylibMotorControlModePOSITION = 4, /// Motor is in velocity control mode + SylibMotorControlModeVelocityAUTO = 5, /// Motor is controlled by internal firmware + SylibMotorControlModePositionAUTO = 6, + SylibMotorControlModeVOLTAGE = 7 /// Motor is set to a specific voltage +} SylibMotorControlMode; + +typedef enum { + SylibAddrledControlModeOFF = 0, + SylibAddrledControlModeMANUAL = 1, + SylibAddrledControlModePULSE = 2, + SylibAddrledControlModeCYCLE = 3 +} SylibAddrledControlMode; +} // namespace sylib \ No newline at end of file diff --git a/include/sylib/system.hpp b/include/sylib/system.hpp new file mode 100644 index 0000000..6901075 --- /dev/null +++ b/include/sylib/system.hpp @@ -0,0 +1,344 @@ +/** + * \file include/sylib/system.hpp + * + * \brief Contains prototypes for functions relating to sylib's background + * processes + * + * 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 https://mozilla.org/MPL/2.0/. + */ + +#pragma once + +#include "env.hpp" +#include "system.hpp" + +namespace sylib { +#ifdef SYLIB_ENV_PROS +typedef pros::Mutex PlatformMutex; +typedef pros::Task PlatformTask; +#elif defined(SYLIB_ENV_VEXCODE) +#define V5_MAX_DEVICE_PORTS 32 +typedef vex::mutex PlatformMutex; +typedef vex::task PlatformTask; +#endif + +/** + * \brief Starts Sylib background processes. Called by the user in initialize() + */ +void initialize(); + +/** + * \brief Delays the current task until a set time + * + * Updates the value at the pointer provided to the current time after the delay + *ends + * + * This is a wrapper for pros::Task::delay_until() if running in a PROS + *enviroment, and a wrapper for vex::this_thread::sleep_until() if in a VEXcode + *enviroment + * + * \param prev_time + * A pointer to a value containing the current system time + * + * \param delta + * The number of milliseconds to pause the task for + **/ +void delay_until(std::uint32_t* const prev_time, const std::uint32_t delta); + +/** + * \brief Delays the current task for a set number of milliseconds + * + * This is a wrapper for pros::delay() if running in a PROS enviroment, + * and a wrapper for vex::this_thread::sleep_for if in a VEXcode enviroment + * + * \param delay + * The number of milliseconds to pause the task for + **/ +void delay(std::uint32_t delay); + +/** + * \brief The current system time + * + * This is a wrapper for pros::millis() if running in a PROS enviroment, + * and a wrapper for vex::timer::system() if in a VEXcode enviroment + * + * \return Milliseconds + **/ +uint32_t millis(); + +/** + * \brief The current system time in microseconds + * + * This value drifts slightly from the system clock over time + * + * \return Microseconds + **/ +uint64_t micros(); + +/** + * \brief Mutex + * + * This is a wrapper for pros::Mutex if running in a PROS enviroment, + * and a wrapper for vex::mutex if running in a VEXcode enviroment + */ +class Mutex : public PlatformMutex { + private: + bool try_lock_for(); + bool try_lock_until(); + + public: + /** + * \brief Locks the mutex + **/ + bool take(); + + /** + * \brief Unlocks the mutex + **/ + bool give(); +}; + +/** + * \brief Task + * + * This is a wrapper for pros::Task if running in a PROS enviroment, + * and a wrapper for vex::task if running in a VEXcode enviroment + */ +class Task : public PlatformTask { + public: + template + Task(F&& func) : PlatformTask(func) {} +}; + +extern Mutex sylib_port_mutexes[V5_MAX_DEVICE_PORTS]; +extern Mutex sylib_controller_mutexes[2]; +using mutex_lock = const std::lock_guard; + +class Device; + +/** + * \brief Sylib System Daemon + * + * This is a singleton this is created when the first object that needs it is. + * + * Takes care of running update functions that need to happen periodically. + */ +class SylibDaemon { + private: + SylibDaemon(); + ~SylibDaemon(); + SylibDaemon(const SylibDaemon&) = delete; + const SylibDaemon& operator=(const SylibDaemon&) = delete; + std::unique_ptr managerTask = nullptr; + static int subTasksCreated; + static std::vector& getLivingSubtasks(); + static int frameCount; + + public: + /** + * \brief Mutex to prevent multiple threads from performing operations + * on important system things at once. + */ + static sylib::Mutex mutex; + + /** + * \brief Gets the single existing SylibDaemon object, + * or creates it if it doesn't exist yet + * + * \return SylibDaemon object + */ + static SylibDaemon& getInstance(); + + /** + * \brief Starts the Sylib daemon. This should be called in initialize() or + * pre_auton() + */ + void startSylibDaemon(); + + /** + * \brief Gets the single existing SylibDaemon object, + * or creates it if it doesn't exist yet + * + * Doesn't lock the mutex. This is used when creating objects in a global + * scope, before RTOS stuff has initialized and mutexes work. + * + * \return SylibDaemon object + */ + static SylibDaemon& getInstanceUnsafe(); + + /** + * \brief The sole function that runs inside a task made specifically for + * the Sylib daemon. Contains an infinite loop that will prevent anything + * else from happening in the thread this is called in. + * + * \return Theoretically 1, but this function should never return on account + * of the while(1) loop + */ + [[noreturn]] static int managerTaskFunction(); + + /** + * \brief Adds a Device object or one of its child classes to the daemon's + * list of objects which need periodic updating + * + * \param objectPointerToSchedule + * A pointer to a Device object + * + * \return The total number of subtasks created + */ + static int createSubTask(sylib::Device* objectPointerToSchedule); + + /** + * \brief Adds a Device object or one of its child classes to the daemon's + * list of objects which need periodic updating + * + * Doesn't lock the mutex. This is used when creating objects in a global + * scope, before RTOS stuff has initialized and mutexes work. + * + * \param objectPointerToSchedule + * A pointer to a Device object + * + * \return The total number of subtasks created + */ + static int createSubTaskUnsafe(sylib::Device* objectPointerToSchedule); + + /** + * \brief Removes a Device object or one of its child classes from the + * daemon's list of objects which need periodic updating + * + * \param objectPointerToSchedule + * A pointer to a Device object + */ + static void removeSubTask(sylib::Device* objectPointerToSchedule); + + /** + * \brief Removes a subtask from the daemon's list + * of objects which need periodic updating + * + * \param idToKill + * The ID number of the subtask to remove + */ + static void removeSubTaskByID(int idToKill); + + /** + * \brief Gets the number of ticks of the Sylib daemon update loop + * + * Should be approximately equal to the system timer divided by 2 + * + * \return Frame count + */ + static uint64_t getFrameCount(); +}; + +/** + * \brief Device Parent Class + * + * Motor and Addrled objects derive from this + */ +class Device { + private: + bool subTaskPaused = false; + int updateFrequency; + int updateOffset; + int subTaskID; + void startSubTask(); + bool idSet = false; + + public: + /** + * \brief Mutex to prevent multiple threads from performing operations + * on important system things at once. + */ + sylib::Mutex mutex; + + /** + * \brief Creates a Device object + * + * \param interval + * The number of frames to wait between updates. A frame is 2ms, so + * this value multiplied by 2 is the approximate time between updates. It + * can be off by +/- 1ms + * + * \param offset + * The number of frames to shift the excecution of the update + * function + */ + Device(int interval = 2, int offset = 0); + + ~Device(); + + /** + * \brief Resumes the device update function + */ + void resumeSubTask(); + + /** + * \brief Pauses the device update function + */ + void pauseSubTask(); + + /** + * \brief Permanently ends the device update function + */ + void killSubTask(); + + /** + * \brief Gets whether or not the device update function is running + * + * \return True or False + */ + bool isSubTaskRunning(); + + /** + * \brief Gets the number of frames delayed between updates + * + * \return Sylib frame count + */ + int getUpdateFrequency(); + + /** + * \brief Gets the number of frames the update function is offset by + * + * \return Sylib frame count + */ + int getUpdateOffset(); + + /** + * \brief Sets the number of frames to wait between updates. + * + * \param interval + * Sylib frame count + */ + void setUpdateFrequency(int interval); + + /** + * \brief Sets the number of frames to shift the excecution of the update + * function + * + * \param interval + * Sylib frame count + */ + void setUpdateOffset(int offset); + + /** + * \brief Gets the subtask ID of the device's update function + * + * \return ID number + */ + int getSubTaskID(); + + /** + * \brief Default device update function. This is overridden by classes that + * inherit from Device for their own specific needs. + */ + virtual void update() = 0; + + /** + * \brief Gets whether or not the device update function is paused + * + * \return True or False + */ + bool getSubTaskPaused(); +}; +} // namespace sylib \ No newline at end of file diff --git a/project.pros b/project.pros index bf78daa..d2c4751 100644 --- a/project.pros +++ b/project.pros @@ -306,6 +306,147 @@ "target": "v5", "user_files": [], "version": "4.8.0" + }, + "sylib": { + "location": "C:\\Users\\emile\\AppData\\Roaming\\PROS\\templates\\sylib@1.0.0", + "metadata": { + "origin": "local" + }, + "name": "sylib", + "py/object": "pros.conductor.templates.local_template.LocalTemplate", + "supported_kernels": "^3.7.2", + "system_files": [ + "include\\display\\lv_objx\\lv_win.h", + "include\\pros\\motors.hpp", + "include\\display\\lv_hal\\lv_hal_indev.h", + "include\\display\\lv_objx\\lv_chart.h", + "include\\display\\lv_objx\\lv_slider.h", + "include\\display\\lv_objx\\lv_label.h", + "include\\display\\lv_objx\\lv_cb.h", + "include\\display\\lv_themes\\lv_theme_default.h", + "include\\pros\\adi.hpp", + "include\\display\\lv_objx\\lv_roller.h", + "include\\display\\lv_draw\\lv_draw_line.h", + "include\\display\\lv_objx\\lv_btnm.h", + "include\\display\\lv_themes\\lv_theme_material.h", + "include\\pros\\colors.hpp", + "include\\pros\\imu.h", + "include\\display\\lv_objx\\lv_img.h", + "include\\pros\\screen.hpp", + "include\\display\\lv_misc\\lv_circ.h", + "include\\pros\\adi.h", + "include\\pros\\screen.h", + "include\\display\\lv_objx\\lv_preload.h", + "include\\display\\lv_core\\lv_indev.h", + "include\\display\\lv_misc\\lv_math.h", + "include\\display\\lv_objx\\lv_table.h", + "include\\pros\\llemu.hpp", + "include\\display\\lv_core\\lv_refr.h", + "include\\display\\lv_themes\\lv_theme_nemo.h", + "include\\sylib\\addrled.hpp", + "include\\display\\lv_misc\\lv_txt.h", + "include\\display\\lv_objx\\lv_objx_templ.h", + "include\\pros\\distance.hpp", + "include\\display\\lv_misc\\lv_color.h", + "include\\display\\lv_draw\\lv_draw.h", + "include\\display\\lv_objx\\lv_tileview.h", + "include\\display\\lv_objx\\lv_ddlist.h", + "include\\pros\\imu.hpp", + "include\\pros\\rotation.h", + "include\\display\\lv_objx\\lv_arc.h", + "include\\display\\lv_objx\\lv_kb.h", + "include\\display\\lv_draw\\lv_draw_rbasic.h", + "include\\pros\\vision.h", + "include\\display\\lv_draw\\lv_draw_img.h", + "include\\display\\lv_misc\\lv_symbol_def.h", + "include\\display\\lv_misc\\lv_font.h", + "include\\pros\\vision.hpp", + "include\\display\\lv_hal\\lv_hal.h", + "include\\display\\lv_objx\\lv_imgbtn.h", + "include\\display\\lv_themes\\lv_theme_zen.h", + "include\\display\\lv_objx\\lv_led.h", + "include\\display\\lv_themes\\lv_theme_alien.h", + "include\\display\\lv_core\\lv_style.h", + "include\\pros\\link.hpp", + "include\\display\\lv_objx\\lv_gauge.h", + "include\\pros\\link.h", + "include\\display\\lv_themes\\lv_theme_mono.h", + "include\\display\\lv_hal\\lv_hal_disp.h", + "include\\display\\lv_misc\\lv_templ.h", + "include\\sylib\\env.hpp", + "include\\display\\lv_objx\\lv_spinbox.h", + "include\\pros\\apix.h", + "include\\display\\lv_conf.h", + "include\\pros\\gps.h", + "include\\pros\\rtos.h", + "include\\display\\lv_objx\\lv_tabview.h", + "include\\pros\\optical.hpp", + "include\\display\\lv_misc\\lv_task.h", + "include\\display\\lv_objx\\lv_line.h", + "include\\sylib\\motor.hpp", + "include\\pros\\llemu.h", + "include\\display\\lv_core\\lv_obj.h", + "include\\display\\lv_objx\\lv_page.h", + "include\\display\\lv_misc\\lv_anim.h", + "include\\sylib\\sylib.hpp", + "include\\display\\lv_objx\\lv_list.h", + "include\\display\\lv_objx\\lv_lmeter.h", + "include\\display\\lv_themes\\lv_theme.h", + "include\\display\\lv_misc\\lv_ufs.h", + "firmware\\sylib.a", + "include\\display\\lv_fonts\\lv_font_builtin.h", + "include\\display\\lv_objx\\lv_sw.h", + "include\\display\\lv_draw\\lv_draw_rect.h", + "include\\pros\\misc.hpp", + "include\\pros\\misc.h", + "include\\pros\\serial.h", + "include\\display\\lv_draw\\lv_draw_arc.h", + "include\\display\\lv_hal\\lv_hal_tick.h", + "include\\pros\\error.h", + "include\\display\\lv_themes\\lv_theme_night.h", + "include\\display\\lv_core\\lv_vdb.h", + "include\\pros\\motors.h", + "include\\pros\\ext_adi.h", + "include\\pros\\rtos.hpp", + "include\\display\\lvgl.h", + "include\\display\\lv_misc\\lv_gc.h", + "include\\display\\lv_objx\\lv_ta.h", + "include\\display\\lv_core\\lv_lang.h", + "include\\display\\lv_draw\\lv_draw_vbasic.h", + "include\\display\\lv_core\\lv_group.h", + "include\\sylib\\pros_includes.h", + "include\\pros\\serial.hpp", + "include\\display\\lv_misc\\lv_fs.h", + "include\\display\\lv_misc\\lv_mem.h", + "include\\display\\lv_objx\\lv_bar.h", + "include\\sylib\\math.hpp", + "include\\api.h", + "include\\display\\lv_draw\\lv_draw_label.h", + "include\\sylib\\system.hpp", + "include\\display\\lv_objx\\lv_cont.h", + "include\\display\\lv_objx\\lv_canvas.h", + "include\\pros\\distance.h", + "include\\pros\\gps.hpp", + "include\\sylib\\sylib_apitypes.hpp", + "include\\display\\lv_conf_checker.h", + "include\\display\\lv_draw\\lv_draw_triangle.h", + "include\\pros\\rotation.hpp", + "include\\display\\lv_objx\\lv_mbox.h", + "include\\pros\\optical.h", + "include\\pros\\colors.h", + "include\\display\\lv_objx\\lv_btn.h", + "include\\display\\lv_version.h", + "include\\display\\lv_misc\\lv_ll.h", + "include\\display\\lv_themes\\lv_theme_templ.h", + "include\\main.h", + "include\\display\\lv_objx\\lv_calendar.h", + "include\\display\\lv_misc\\lv_log.h", + "include\\pros\\api_legacy.h", + "include\\display\\lv_misc\\lv_area.h" + ], + "target": "v5", + "user_files": [], + "version": "1.0.0" } }, "upload_options": {} diff --git a/src/main.cpp b/src/main.cpp index 47d96c0..7fb697c 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -154,18 +154,20 @@ void initialize() { pros::lcd::register_btn1_cb(on_center_button); pros::delay(100); - up.reset(); + /*up.reset(); up2.reset(); sideways.reset(); imu1.initialize(); - pros::delay(100); - odom1 = odom(); + pros::delay(100);*/ + //odom1 = odom(); - odom1.position = Cartesian(88.5_in, 7.5_in); + //odom1.position = Cartesian(88.5_in, 7.5_in); - pros::Task myTask(odomTask); - pros::Task myFlywheelTask(flywheelTask); + //pros::Task myTask(odomTask); + //pros::Task myFlywheelTask(flywheelTask); indexer.set_value(false); + + sylib::initialize(); } /** @@ -347,8 +349,32 @@ double normRightY() { // } +void opcontrol(){ +// Create an addrled object + auto addrled = sylib::Addrled(22, 2, 26); + + // Set the LED strip to a gradient in HSV color space + // that displays a full range of hues + addrled.gradient(0xFF0000, 0xFFEE00); + + // Cycle the colors at speed 10 + addrled.cycle(*addrled, 10); + + + + // Store the time at the start of the loop + std::uint32_t clock = sylib::millis(); + while (true) { + //printf("looping\n"); + // 10ms delay to allow other tasks to run + //addrled.pulse(0x0000FF, 4, 52); + sylib::delay_until(&clock, 500); + } +} + float diagnosticTimer; -void opcontrol() { +void opcontrol2() { + //pros::delay(100); //up.reset(); //sideways.reset(); diff --git a/sylib@1.0.0.zip b/sylib@1.0.0.zip new file mode 100644 index 0000000..d957fe5 Binary files /dev/null and b/sylib@1.0.0.zip differ