add lemlib mod

CLOUDS/firmware/asset.mk

@@ -0,0 +1,10 @@
+ASSET_FILES=$(wildcard static/*)
+ASSET_OBJ=$(addprefix $(BINDIR)/, $(addsuffix .o, $(ASSET_FILES)) )
+
+GETALLOBJ=$(sort $(call ASMOBJ,$1) $(call COBJ,$1) $(call CXXOBJ,$1)) $(ASSET_OBJ)
+
+.SECONDEXPANSION:
+$(ASSET_OBJ): $$(patsubst bin/%,%,$$(basename $$@))
+	$(VV)mkdir -p $(BINDIR)/static
+	@echo "ASSET $@"
+	$(VV)$(OBJCOPY) -I binary -O elf32-littlearm -B arm $^ $@

CLOUDS/include/fmt/args.h

@@ -0,0 +1,207 @@
+// Formatting library for C++ - dynamic argument lists
+//
+// Copyright (c) 2012 - present, Victor Zverovich
+// All rights reserved.
+//
+// For the license information refer to format.h.
+
+#ifndef FMT_ARGS_H_
+#define FMT_ARGS_H_
+
+#include <functional> // std::reference_wrapper
+#include <memory> // std::unique_ptr
+#include <vector>
+
+#include "core.h"
+
+FMT_BEGIN_NAMESPACE
+
+namespace detail {
+
+template <typename T> struct is_reference_wrapper : std::false_type {};
+
+template <typename T> struct is_reference_wrapper<std::reference_wrapper<T>> : std::true_type {};
+
+template <typename T> const T& unwrap(const T& v) { return v; }
+
+template <typename T> const T& unwrap(const std::reference_wrapper<T>& v) { return static_cast<const T&>(v); }
+
+class dynamic_arg_list {
+        // Workaround for clang's -Wweak-vtables. Unlike for regular classes, for
+        // templates it doesn't complain about inability to deduce single translation
+        // unit for placing vtable. So storage_node_base is made a fake template.
+        template <typename = void> struct node {
+                virtual ~node() = default;
+                std::unique_ptr<node<>> next;
+        };
+
+        template <typename T> struct typed_node : node<> {
+                T value;
+
+                template <typename Arg> FMT_CONSTEXPR typed_node(const Arg& arg) : value(arg) {}
+
+                template <typename Char> FMT_CONSTEXPR typed_node(const basic_string_view<Char>& arg)
+                    : value(arg.data(), arg.size()) {}
+        };
+
+        std::unique_ptr<node<>> head_;
+    public:
+        template <typename T, typename Arg> const T& push(const Arg& arg) {
+            auto new_node = std::unique_ptr<typed_node<T>>(new typed_node<T>(arg));
+            auto& value = new_node->value;
+            new_node->next = std::move(head_);
+            head_ = std::move(new_node);
+            return value;
+        }
+};
+} // namespace detail
+
+/**
+  \rst
+  A dynamic version of `fmt::format_arg_store`.
+  It's equipped with a storage to potentially temporary objects which lifetimes
+  could be shorter than the format arguments object.
+
+  It can be implicitly converted into `~fmt::basic_format_args` for passing
+  into type-erased formatting functions such as `~fmt::vformat`.
+  \endrst
+ */
+template <typename Context> class dynamic_format_arg_store
+#if FMT_GCC_VERSION && FMT_GCC_VERSION < 409
+    // Workaround a GCC template argument substitution bug.
+    : public basic_format_args<Context>
+#endif
+{
+    private:
+        using char_type = typename Context::char_type;
+
+        template <typename T> struct need_copy {
+                static constexpr detail::type mapped_type = detail::mapped_type_constant<T, Context>::value;
+
+                enum {
+                    value = !(detail::is_reference_wrapper<T>::value ||
+                              std::is_same<T, basic_string_view<char_type>>::value ||
+                              std::is_same<T, detail::std_string_view<char_type>>::value ||
+                              (mapped_type != detail::type::cstring_type && mapped_type != detail::type::string_type &&
+                               mapped_type != detail::type::custom_type))
+                };
+        };
+
+        template <typename T> using stored_type =
+            conditional_t<std::is_convertible<T, std::basic_string<char_type>>::value &&
+                              !detail::is_reference_wrapper<T>::value,
+                          std::basic_string<char_type>, T>;
+
+        // Storage of basic_format_arg must be contiguous.
+        std::vector<basic_format_arg<Context>> data_;
+        std::vector<detail::named_arg_info<char_type>> named_info_;
+
+        // Storage of arguments not fitting into basic_format_arg must grow
+        // without relocation because items in data_ refer to it.
+        detail::dynamic_arg_list dynamic_args_;
+
+        friend class basic_format_args<Context>;
+
+        unsigned long long get_types() const {
+            return detail::is_unpacked_bit | data_.size() |
+                   (named_info_.empty() ? 0ULL : static_cast<unsigned long long>(detail::has_named_args_bit));
+        }
+
+        const basic_format_arg<Context>* data() const { return named_info_.empty() ? data_.data() : data_.data() + 1; }
+
+        template <typename T> void emplace_arg(const T& arg) { data_.emplace_back(detail::make_arg<Context>(arg)); }
+
+        template <typename T> void emplace_arg(const detail::named_arg<char_type, T>& arg) {
+            if (named_info_.empty()) {
+                constexpr const detail::named_arg_info<char_type>* zero_ptr {nullptr};
+                data_.insert(data_.begin(), {zero_ptr, 0});
+            }
+            data_.emplace_back(detail::make_arg<Context>(detail::unwrap(arg.value)));
+            auto pop_one = [](std::vector<basic_format_arg<Context>>* data) { data->pop_back(); };
+            std::unique_ptr<std::vector<basic_format_arg<Context>>, decltype(pop_one)> guard {&data_, pop_one};
+            named_info_.push_back({arg.name, static_cast<int>(data_.size() - 2u)});
+            data_[0].value_.named_args = {named_info_.data(), named_info_.size()};
+            guard.release();
+        }
+    public:
+        constexpr dynamic_format_arg_store() = default;
+
+        /**
+          \rst
+          Adds an argument into the dynamic store for later passing to a formatting
+          function.
+
+          Note that custom types and string types (but not string views) are copied
+          into the store dynamically allocating memory if necessary.
+
+          **Example**::
+
+            fmt::dynamic_format_arg_store<fmt::format_context> store;
+            store.push_back(42);
+            store.push_back("abc");
+            store.push_back(1.5f);
+            std::string result = fmt::vformat("{} and {} and {}", store);
+          \endrst
+        */
+        template <typename T> void push_back(const T& arg) {
+            if (detail::const_check(need_copy<T>::value)) emplace_arg(dynamic_args_.push<stored_type<T>>(arg));
+            else emplace_arg(detail::unwrap(arg));
+        }
+
+        /**
+          \rst
+          Adds a reference to the argument into the dynamic store for later passing to
+          a formatting function.
+
+          **Example**::
+
+            fmt::dynamic_format_arg_store<fmt::format_context> store;
+            char band[] = "Rolling Stones";
+            store.push_back(std::cref(band));
+            band[9] = 'c'; // Changing str affects the output.
+            std::string result = fmt::vformat("{}", store);
+            // result == "Rolling Scones"
+          \endrst
+        */
+        template <typename T> void push_back(std::reference_wrapper<T> arg) {
+            static_assert(need_copy<T>::value, "objects of built-in types and string views are always copied");
+            emplace_arg(arg.get());
+        }
+
+        /**
+          Adds named argument into the dynamic store for later passing to a formatting
+          function. ``std::reference_wrapper`` is supported to avoid copying of the
+          argument. The name is always copied into the store.
+        */
+        template <typename T> void push_back(const detail::named_arg<char_type, T>& arg) {
+            const char_type* arg_name = dynamic_args_.push<std::basic_string<char_type>>(arg.name).c_str();
+            if (detail::const_check(need_copy<T>::value)) {
+                emplace_arg(fmt::arg(arg_name, dynamic_args_.push<stored_type<T>>(arg.value)));
+            } else {
+                emplace_arg(fmt::arg(arg_name, arg.value));
+            }
+        }
+
+        /** Erase all elements from the store */
+        void clear() {
+            data_.clear();
+            named_info_.clear();
+            dynamic_args_ = detail::dynamic_arg_list();
+        }
+
+        /**
+          \rst
+          Reserves space to store at least *new_cap* arguments including
+          *new_cap_named* named arguments.
+          \endrst
+        */
+        void reserve(size_t new_cap, size_t new_cap_named) {
+            FMT_ASSERT(new_cap >= new_cap_named, "Set of arguments includes set of named arguments");
+            data_.reserve(new_cap);
+            named_info_.reserve(new_cap_named);
+        }
+};
+
+FMT_END_NAMESPACE
+
+#endif // FMT_ARGS_H_

CLOUDS/include/lemlib/api.hpp

@@ -0,0 +1,19 @@
+/**
+ * @file include/lemlib/api.hpp
+ * @author LemLib Team
+ * @brief LemLib API header file. Include this in your source files to use the library.
+ * @version 0.4.5
+ * @date 2023-01-27
+ *
+ * @copyright Copyright (c) 2023
+ *
+ */
+#pragma once
+
+#include "lemlib/util.hpp"
+#include "lemlib/pid.hpp"
+#include "lemlib/pose.hpp"
+#include "lemlib/chassis/trackingWheel.hpp"
+#include "lemlib/chassis/chassis.hpp"
+
+#include "lemlib/logger/logger.hpp"

CLOUDS/include/lemlib/asset.hpp

@@ -0,0 +1,18 @@
+#pragma once
+
+#include <stdint.h>
+#include <cstddef>
+
+extern "C" {
+
+typedef struct __attribute__((__packed__)) _asset {
+        uint8_t* buf;
+        size_t size;
+} asset;
+}
+
+#define ASSET(x)                                                                                                       \
+    extern "C" {                                                                                                       \
+    extern uint8_t _binary_static_##x##_start[], _binary_static_##x##_size[];                                          \
+    static asset x = {_binary_static_##x##_start, (size_t)_binary_static_##x##_size};                                  \
+    }

CLOUDS/include/lemlib/chassis/chassis.hpp

@@ -0,0 +1,299 @@
+/**
+ * @file include/lemlib/chassis/chassis.hpp
+ * @author LemLib Team
+ * @brief Chassis class declarations
+ * @version 0.4.5
+ * @date 2023-01-23
+ *
+ * @copyright Copyright (c) 2023
+ *
+ */
+
+#pragma once
+
+#include <functional>
+#include "pros/rtos.hpp"
+#include "pros/motors.hpp"
+#include "pros/imu.hpp"
+#include "lemlib/asset.hpp"
+#include "lemlib/chassis/trackingWheel.hpp"
+#include "lemlib/pose.hpp"
+
+namespace lemlib {
+/**
+ * @brief Struct containing all the sensors used for odometry
+ *
+ */
+struct OdomSensors {
+        /**
+         * The sensors are stored in a struct so that they can be easily passed to the chassis class
+         * The variables are pointers so that they can be set to nullptr if they are not used
+         * Otherwise the chassis class would have to have a constructor for each possible combination of sensors
+         *
+         * @param vertical1 pointer to the first vertical tracking wheel
+         * @param vertical2 pointer to the second vertical tracking wheel
+         * @param horizontal1 pointer to the first horizontal tracking wheel
+         * @param horizontal2 pointer to the second horizontal tracking wheel
+         * @param imu pointer to the IMU
+         */
+        OdomSensors(TrackingWheel* vertical1, TrackingWheel* vertical2, TrackingWheel* horizontal1,
+                    TrackingWheel* horizontal2, pros::Imu* imu);
+        TrackingWheel* vertical1;
+        TrackingWheel* vertical2;
+        TrackingWheel* horizontal1;
+        TrackingWheel* horizontal2;
+        pros::Imu* imu;
+};
+
+/**
+ * @brief Struct containing constants for a chassis controller
+ *
+ */
+struct ControllerSettings {
+        /**
+         * The constants are stored in a struct so that they can be easily passed to the chassis class
+         * Set a constant to 0 and it will be ignored
+         *
+         * @param kP proportional constant for the chassis controller
+         * @param kD derivative constant for the chassis controller
+         * @param smallError the error at which the chassis controller will switch to a slower control loop
+         * @param smallErrorTimeout the time the chassis controller will wait before switching to a slower control loop
+         * @param largeError the error at which the chassis controller will switch to a faster control loop
+         * @param largeErrorTimeout the time the chassis controller will wait before switching to a faster control loop
+         * @param slew the maximum acceleration of the chassis controller
+         */
+        ControllerSettings(float kP, float kD, float smallError, float smallErrorTimeout, float largeError,
+                           float largeErrorTimeout, float slew);
+        float kP;
+        float kD;
+        float smallError;
+        float smallErrorTimeout;
+        float largeError;
+        float largeErrorTimeout;
+        float slew;
+};
+
+/**
+ * @brief Struct containing constants for a drivetrain
+ *
+ */
+struct Drivetrain {
+        /**
+         * The constants are stored in a struct so that they can be easily passed to the chassis class
+         * Set a constant to 0 and it will be ignored
+         *
+         * @param leftMotors pointer to the left motors
+         * @param rightMotors pointer to the right motors
+         * @param trackWidth the track width of the robot
+         * @param wheelDiameter the diameter of the wheel used on the drivetrain
+         * @param rpm the rpm of the wheels
+         * @param chasePower higher values make the robot move faster but causes more overshoot on turns
+         */
+        Drivetrain(pros::MotorGroup* leftMotors, pros::MotorGroup* rightMotors, float trackWidth, float wheelDiameter,
+                   float rpm, float chasePower);
+        pros::Motor_Group* leftMotors;
+        pros::Motor_Group* rightMotors;
+        float trackWidth;
+        float wheelDiameter;
+        float rpm;
+        float chasePower;
+};
+
+/**
+ * @brief Function pointer type for drive curve functions.
+ * @param input The control input in the range [-127, 127].
+ * @param scale The scaling factor, which can be optionally ignored.
+ * @return The new value to be used.
+ */
+typedef std::function<float(float, float)> DriveCurveFunction_t;
+
+/**
+ * @brief  Default drive curve. Modifies  the input with an exponential curve. If the input is 127, the function
+ * will always output 127, no matter the value of scale, likewise for -127. This curve was inspired by team
+ * 5225, the Pilons. A Desmos graph of this curve can be found here:
+ * https://www.desmos.com/calculator/rcfjjg83zx
+ * @param input value from -127 to 127
+ * @param scale how steep the curve should be.
+ * @return The new value to be used.
+ */
+float defaultDriveCurve(float input, float scale);
+
+/**
+ * @brief Chassis class
+ *
+ */
+class Chassis {
+    public:
+        /**
+         * @brief Construct a new Chassis
+         *
+         * @param drivetrain drivetrain to be used for the chassis
+         * @param linearSettings settings for the linear controller
+         * @param angularSettings settings for the angular controller
+         * @param sensors sensors to be used for odometry
+         * @param driveCurve drive curve to be used. defaults to `defaultDriveCurve`
+         */
+        Chassis(Drivetrain drivetrain, ControllerSettings linearSettings, ControllerSettings angularSettings,
+                OdomSensors sensors, DriveCurveFunction_t driveCurve = &defaultDriveCurve);
+        /**
+         * @brief Calibrate the chassis sensors
+         *
+         * @param calibrateIMU whether the IMU should be calibrated. true by default
+         */
+        void calibrate(bool calibrateIMU = true);
+        /**
+         * @brief Set the pose of the chassis
+         *
+         * @param x new x value
+         * @param y new y value
+         * @param theta new theta value
+         * @param radians true if theta is in radians, false if not. False by default
+         */
+        void setPose(float x, float y, float theta, bool radians = false);
+        /**
+         * @brief Set the pose of the chassis
+         *
+         * @param pose the new pose
+         * @param radians whether pose theta is in radians (true) or not (false). false by default
+         */
+        void setPose(Pose pose, bool radians = false);
+        /**
+         * @brief Get the pose of the chassis
+         *
+         * @param radians whether theta should be in radians (true) or degrees (false). false by default
+         * @return Pose
+         */
+        Pose getPose(bool radians = false);
+        /**
+         * @brief Get the speed of the robot
+         *
+         * @param radians true for theta in radians, false for degrees. False by default
+         * @return lemlib::Pose
+         */
+        Pose getSpeed(bool radians = false);
+        /**
+         * @brief Get the local speed of the robot
+         *
+         * @param radians true for theta in radians, false for degrees. False by default
+         * @return lemlib::Pose
+         */
+        Pose getLocalSpeed(bool radians = false);
+        /**
+         * @brief Estimate the pose of the robot after a certain amount of time
+         *
+         * @param time time in seconds
+         * @param radians False for degrees, true for radians. False by default
+         * @return lemlib::Pose
+         */
+        Pose estimatePose(float time, bool radians = false);
+        /**
+         * @brief Wait until the robot has traveled a certain distance along the path
+         *
+         * @note Units are in inches if current motion is moveTo or follow, degrees if using turnTo
+         *
+         * @param dist the distance the robot needs to travel before returning
+         */
+        void waitUntil(float dist);
+        /**
+         * @brief Wait until the robot has completed the path
+         *
+         */
+        void waitUntilDone();
+        /**
+         * @brief Turn the chassis so it is facing the target point
+         *
+         * The PID logging id is "angularPID"
+         *
+         * @param x x location
+         * @param y y location
+         * @param timeout longest time the robot can spend moving
+         * @param forwards whether the robot should turn to face the point with the front of the robot. true by
+         * default
+         * @param maxSpeed the maximum speed the robot can turn at. Default is 127
+         * @param async whether the function should be run asynchronously. true by default
+         */
+        void turnTo(float x, float y, int timeout, bool forwards = true, float maxSpeed = 127, bool async = true);
+        /**
+         * @brief Move the chassis towards the target pose
+         *
+         * Uses the boomerang controller
+         *
+         * @param x x location
+         * @param y y location
+         * @param theta target heading in degrees.
+         * @param timeout longest time the robot can spend moving
+         * @param forwards whether the robot should move forwards or backwards. true for forwards (default),
+         * false for backwards
+         * @param chasePower higher values make the robot move faster but causes more overshoot on turns. 0
+         * makes it default to global value
+         * @param lead the lead parameter. Determines how curved the robot will move. 0.6 by default (0 < lead <
+         * 1)
+         * @param maxSpeed the maximum speed the robot can move at. 127 at default
+         * @param async whether the function should be run asynchronously. true by default
+         */
+        void moveToPose(float x, float y, float theta, int timeout, bool forwards = true, float chasePower = 0,
+                        float lead = 0.6, float maxSpeed = 127, bool async = true);
+        /**
+         * @brief Move the chassis towards a target point
+         *
+         * @param x x location
+         * @param y y location
+         * @param timeout longest time the robot can spend moving
+         * @param maxSpeed the maximum speed the robot can move at. 127 by default
+         * @param async whether the function should be run asynchronously. true by default
+         */
+        void moveToPoint(float x, float y, int timeout, bool forwards = true, float maxSpeed = 127, bool async = true);
+        /**
+         * @brief Move the chassis along a path
+         *
+         * @param path the path asset to follow
+         * @param lookahead the lookahead distance. Units in inches. Larger values will make the robot move
+         * faster but will follow the path less accurately
+         * @param timeout the maximum time the robot can spend moving
+         * @param forwards whether the robot should follow the path going forwards. true by default
+         * @param async whether the function should be run asynchronously. true by default
+         */
+        void follow(const asset& path, float lookahead, int timeout, bool forwards = true, bool async = true);
+        /**
+         * @brief Control the robot during the driver control period using the tank drive control scheme. In
+         * this control scheme one joystick axis controls one half of the robot, and another joystick axis
+         * controls another.
+         * @param left speed of the left side of the drivetrain. Takes an input from -127 to 127.
+         * @param right speed of the right side of the drivetrain. Takes an input from -127 to 127.
+         * @param curveGain control how steep the drive curve is. The larger the number, the steeper the curve.
+         * A value of 0 disables the curve entirely.
+         */
+        void tank(int left, int right, float curveGain = 0.0);
+        /**
+         * @brief Control the robot during the driver using the arcade drive control scheme. In this control
+         * scheme one joystick axis controls the forwards and backwards movement of the robot, while the other
+         * joystick axis controls  the robot's turning
+         * @param throttle speed to move forward or backward. Takes an input from -127 to 127.
+         * @param turn speed to turn. Takes an input from -127 to 127.
+         * @param curveGain the scale inputted into the drive curve function. If you are using the default drive
+         * curve, refer to the `defaultDriveCurve` documentation.
+         */
+        void arcade(int throttle, int turn, float curveGain = 0.0);
+        /**
+         * @brief Control the robot during the driver using the curvature drive control scheme. This control
+         * scheme is very similar to arcade drive, except the second joystick axis controls the radius of the
+         * curve that the drivetrain makes, rather than the speed. This means that the driver can accelerate in
+         * a turn without changing the radius of that turn. This control scheme defaults to arcade when forward
+         * is zero.
+         * @param throttle speed to move forward or backward. Takes an input from -127 to 127.
+         * @param turn speed to turn. Takes an input from -127 to 127.
+         * @param curveGain the scale inputted into the drive curve function. If you are using the default drive
+         * curve, refer to the `defaultDriveCurve` documentation.
+         */
+        void curvature(int throttle, int turn, float cureGain = 0.0);
+    private:
+        pros::Mutex mutex;
+        float distTravelled = 0;
+
+        ControllerSettings linearSettings;
+        ControllerSettings angularSettings;
+        Drivetrain drivetrain;
+        OdomSensors sensors;
+        DriveCurveFunction_t driveCurve;
+};
+} // namespace lemlib

CLOUDS/include/lemlib/chassis/odom.hpp

@@ -0,0 +1,72 @@
+/**
+ * @file include/lemlib/chassis/odom.hpp
+ * @author LemLib Team
+ * @brief This is the header file for the odom.cpp file. Its not meant to be used directly, only through the chassis
+ * class
+ * @version 0.4.5
+ * @date 2023-01-23
+ *
+ * @copyright Copyright (c) 2023
+ *
+ */
+
+#pragma once
+
+#include "lemlib/chassis/chassis.hpp"
+#include "lemlib/pose.hpp"
+
+namespace lemlib {
+/**
+ * @brief Set the sensors to be used for odometry
+ *
+ * @param sensors the sensors to be used
+ * @param drivetrain drivetrain to be used
+ */
+void setSensors(lemlib::OdomSensors sensors, lemlib::Drivetrain drivetrain);
+/**
+ * @brief Get the pose of the robot
+ *
+ * @param radians true for theta in radians, false for degrees. False by default
+ * @return Pose
+ */
+Pose getPose(bool radians = false);
+/**
+ * @brief Set the Pose of the robot
+ *
+ * @param pose the new pose
+ * @param radians true if theta is in radians, false if in degrees. False by default
+ */
+void setPose(Pose pose, bool radians = false);
+/**
+ * @brief Get the speed of the robot
+ *
+ * @param radians true for theta in radians, false for degrees. False by default
+ * @return lemlib::Pose
+ */
+Pose getSpeed(bool radians = false);
+/**
+ * @brief Get the local speed of the robot
+ *
+ * @param radians true for theta in radians, false for degrees. False by default
+ * @return lemlib::Pose
+ */
+Pose getLocalSpeed(bool radians = false);
+/**
+ * @brief Estimate the pose of the robot after a certain amount of time
+ *
+ * @param time time in seconds
+ * @param radians False for degrees, true for radians. False by default
+ * @return lemlib::Pose
+ */
+Pose estimatePose(float time, bool radians = false);
+/**
+ * @brief Update the pose of the robot
+ *
+ */
+void update();
+/**
+ * @brief Initialize the odometry system
+ *
+ */
+void init();
+} // namespace lemlib

CLOUDS/include/lemlib/chassis/trackingWheel.hpp

@@ -0,0 +1,99 @@
+/**
+ * @file include/lemlib/chassis/trackingWheel.hpp
+ * @author LemLib Team
+ * @brief tracking wheel class declarations
+ * @version 0.4.5
+ * @date 2023-01-23
+ *
+ * @copyright Copyright (c) 2023
+ *
+ */
+
+#pragma once
+
+#include "pros/motors.hpp"
+#include "pros/adi.hpp"
+#include "pros/rotation.hpp"
+
+namespace lemlib {
+
+/**
+ * @brief A namespace representing the size of omniwheels.
+ */
+namespace Omniwheel {
+constexpr float NEW_275 = 2.75;
+constexpr float OLD_275 = 2.75;
+constexpr float NEW_275_HALF = 2.744;
+constexpr float OLD_275_HALF = 2.74;
+constexpr float NEW_325 = 3.25;
+constexpr float OLD_325 = 3.25;
+constexpr float NEW_325_HALF = 3.246;
+constexpr float OLD_325_HALF = 3.246;
+constexpr float NEW_4 = 4;
+constexpr float OLD_4 = 4.18;
+constexpr float NEW_4_HALF = 3.995;
+constexpr float OLD_4_HALF = 4.175;
+} // namespace Omniwheel
+
+class TrackingWheel {
+    public:
+        /**
+         * @brief Create a new tracking wheel
+         *
+         * @param encoder the optical shaft encoder to use
+         * @param wheelDiameter the diameter of the wheel
+         * @param distance distance between the tracking wheel and the center of rotation in inches
+         * @param gearRatio gear ratio of the tracking wheel, defaults to 1
+         */
+        TrackingWheel(pros::ADIEncoder* encoder, float wheelDiameter, float distance, float gearRatio = 1);
+        /**
+         * @brief Create a new tracking wheel
+         *
+         * @param encoder the v5 rotation sensor to use
+         * @param wheelDiameter the diameter of the wheel
+         * @param distance distance between the tracking wheel and the center of rotation in inches
+         * @param gearRatio gear ratio of the tracking wheel, defaults to 1
+         */
+        TrackingWheel(pros::Rotation* encoder, float wheelDiameter, float distance, float gearRatio = 1);
+        /**
+         * @brief Create a new tracking wheel
+         *
+         * @param motors the motor group to use
+         * @param wheelDiameter the diameter of the wheel
+         * @param distance half the track width of the drivetrain in inches
+         * @param rpm theoretical maximum rpm of the drivetrain wheels
+         */
+        TrackingWheel(pros::Motor_Group* motors, float wheelDiameter, float distance, float rpm);
+        /**
+         * @brief Reset the tracking wheel position to 0
+         *
+         */
+        void reset();
+        /**
+         * @brief Get the distance traveled by the tracking wheel
+         *
+         * @return float distance traveled in inches
+         */
+        float getDistanceTraveled();
+        /**
+         * @brief Get the offset of the tracking wheel from the center of rotation
+         *
+         * @return float offset in inches
+         */
+        float getOffset();
+        /**
+         * @brief Get the type of tracking wheel
+         *
+         * @return int - 1 if motor group, 0 otherwise
+         */
+        int getType();
+    private:
+        float diameter;
+        float distance;
+        float rpm;
+        pros::ADIEncoder* encoder = nullptr;
+        pros::Rotation* rotation = nullptr;
+        pros::Motor_Group* motors = nullptr;
+        float gearRatio = 1;
+};
+} // namespace lemlib

CLOUDS/include/lemlib/logger/baseSink.hpp

@@ -0,0 +1,209 @@
+#pragma once
+
+#include <initializer_list>
+#include "pros/rtos.hpp"
+
+#define FMT_HEADER_ONLY
+#include "fmt/core.h"
+#include "fmt/args.h"
+
+#include "lemlib/logger/message.hpp"
+
+namespace lemlib {
+/**
+ * @brief A base for any sink in LemLib to implement.
+ *
+ * Sinks are LemLib's abstraction for destinations that logged messages can be sent to. They are the backbone of the
+ * logging implementation. A sink could send information to anything, to stdout, to a file, or even the UI on the
+ * brain screen.
+ */
+class BaseSink {
+    public:
+        BaseSink() = default;
+
+        /**
+         * @brief Construct a new combined sink
+         *
+         * <h3> Example Usage </h3>
+         * @code
+         * lemlib::BaseSink combinedSink({lemlib::telemetrySink(), lemlib::infoSink()});
+         * combinedSink.info("This will be sent to both sinks!");
+         * @endcode
+         *
+         * @param sinks The sinks that will have messages sent to them when
+         */
+        BaseSink(std::initializer_list<std::shared_ptr<BaseSink>> sinks);
+
+        /**
+         * @brief Set the lowest level.
+         * If this is a combined sink, this operation will
+         * apply for all the parent sinks.
+         * @param level
+         *
+         * If messages are logged that are below the lowest level, they will be ignored.
+         * The hierarchy of the levels is as follows:
+         * - INFO
+         * - DEBUG
+         * - WARN
+         * - ERROR
+         * - FATAL
+         */
+        void setLowestLevel(Level level);
+
+        /**
+         * @brief Log a message at the given level
+         * If this is a combined sink, this operation will
+         * apply for all the parent sinks.
+         *
+         * @tparam T
+         * @param level The level at which to send the message.
+         * @param format The format that the message will use. Use "{}" as placeholders.
+         * @param args The values that will be substituted into the placeholders in the format.
+         *
+         * <h3> Example Usage </h3>
+         * @code
+         * sink.log(lemlib::Level::INFO, "{} from the logger!", "Hello");
+         * @endcode
+
+         */
+        template <typename... T> void log(Level level, fmt::format_string<T...> format, T&&... args) {
+            if (!sinks.empty()) {
+                for (std::shared_ptr<BaseSink> sink : sinks) { sink->log(level, format, std::forward<T>(args)...); }
+                return;
+            }
+
+            if (level < lowestLevel) { return; }
+
+            // substitute the user's arguments into the format.
+            std::string messageString = fmt::format(format, std::forward<T>(args)...);
+
+            Message message = Message {.level = level, .time = pros::millis()};
+
+            // get the arguments
+            fmt::dynamic_format_arg_store<fmt::format_context> formattingArgs = getExtraFormattingArgs(message);
+
+            formattingArgs.push_back(fmt::arg("time", message.time));
+            formattingArgs.push_back(fmt::arg("level", message.level));
+            formattingArgs.push_back(fmt::arg("message", messageString));
+
+            std::string formattedString = fmt::vformat(logFormat, std::move(formattingArgs));
+            message.message = std::move(formattedString);
+            sendMessage(std::move(message));
+        }
+
+        /**
+         * @brief Log a message at the debug level.
+         * If this is a combined sink, this operation will
+         * apply for all the parent sinks.
+         * @tparam T
+         * @param format
+         * @param args
+         */
+        template <typename... T> void debug(fmt::format_string<T...> format, T&&... args) {
+            log(Level::DEBUG, format, std::forward<T>(args)...);
+        }
+
+        /**
+         * @brief Log a message at the info level
+         * If this is a combined sink, this operation will
+         * apply for all the parent sinks.
+         * @tparam T
+         * @param format
+         * @param args
+         */
+        template <typename... T> void info(fmt::format_string<T...> format, T&&... args) {
+            log(Level::INFO, format, std::forward<T>(args)...);
+        }
+
+        /**
+         * @brief Log a message at the warn level
+         * If this is a combined sink, this operation will
+         * apply for all the parent sinks.
+         * @tparam T
+         * @param format
+         * @param args
+         */
+        template <typename... T> void warn(fmt::format_string<T...> format, T&&... args) {
+            log(Level::WARN, format, std::forward<T>(args)...);
+        }
+
+        /**
+         * @brief Log a message at the error level.
+         * If this is a combined sink, this operation will
+         * apply for all the parent sinks.
+         * @tparam T
+         * @param format
+         * @param args
+         */
+        template <typename... T> void error(fmt::format_string<T...> format, T&&... args) {
+            log(Level::ERROR, format, std::forward<T>(args)...);
+        }
+
+        /**
+         * @brief Log a message at the fatal level
+         * If this is a combined sink, this operation will
+         * apply for all the parent sinks.
+         * @tparam T
+         * @param format
+         * @param args
+         */
+        template <typename... T> void fatal(fmt::format_string<T...> format, T&&... args) {
+            log(Level::FATAL, format, std::forward<T>(args)...);
+        }
+    protected:
+        /**
+         * @brief Log the given message
+         *
+         * @param message
+         */
+        virtual void sendMessage(const Message& message);
+
+        /**
+         * @brief Set the format of messages that the sink sends
+         *
+         * @param format
+         *
+         * Changing the format of the sink changes the way each logged message looks. The following named formatting
+         * specifiers can be used:
+         * - {time} The time the message was sent in milliseconds since the program started.
+         * - {level} The level of the logged message.
+         * - {message} The message itself.
+         *
+         * <h3> Example Usage </h3>
+         * @code
+         * infoSink()->setFormat("[LemLib] -- {time} -- {level}: {Message}");
+         * infoSink()->info("hello");
+         * // This will be formatted as:
+         * // "[LemLIb] -- 10 -- INFO:  hello"
+         * @endcode
+         */
+        void setFormat(const std::string& format);
+
+        /**
+         * @brief Get the extra named arguments for formatting
+         *
+         * Can be overridden to add extra named arguments to the sink's format.
+         * <h3> Example Usage </h3>
+         *
+         * The following code would add a {zero} formatting argument which could be used in setFormat method of this
+         * sink.
+         *
+         * @code
+         * fmt::dynamic_format_arg_store<fmt::format_context>
+         * ExampleSink::getExtraFormattingArgs(const Message& messageInfo) {
+         *     fmt::dynamic_format_arg_store<fmt::format_context> args;
+         *      args.push_back(fmt::arg("zero", 0);
+         *      return args;
+         * }
+         * @endcode
+         *
+         * @return fmt::dynamic_format_arg_store<fmt::format_context>
+         */
+        virtual fmt::dynamic_format_arg_store<fmt::format_context> getExtraFormattingArgs(const Message& messageInfo);
+    private:
+        Level lowestLevel = Level::DEBUG;
+        std::string logFormat;
+
+        std::vector<std::shared_ptr<BaseSink>> sinks {};
+};
+} // namespace lemlib

CLOUDS/include/lemlib/logger/buffer.hpp

@@ -0,0 +1,72 @@
+#pragma once
+
+#include <deque>
+#include <functional>
+#include <string>
+
+#include "pros/rtos.hpp"
+
+namespace lemlib {
+/**
+ * @brief A buffer implementation
+ *
+ * Asynchronously processes a backlog of strings at a given rate. The strings are processed in a first in last out
+ * order.
+ */
+class Buffer {
+    public:
+        /**
+         * @brief Construct a new Buffer object
+         *
+         */
+        Buffer(std::function<void(const std::string&)> bufferFunc);
+
+        /**
+         * @brief Destroy the Buffer object
+         *
+         */
+        ~Buffer();
+
+        Buffer(const Buffer&) = delete;
+        Buffer& operator=(const Buffer&) = delete;
+
+        /**
+         * @brief Push to the buffer
+         *
+         * @param bufferData
+         */
+        void pushToBuffer(const std::string& bufferData);
+
+        /**
+         * @brief Set the rate of the sink
+         *
+         * @param rate
+         */
+        void setRate(uint32_t rate);
+
+        /**
+         * @brief Check to see if the internal buffer is empty
+         *
+         */
+        bool buffersEmpty();
+    private:
+        /**
+         * @brief The function that will be run inside of the buffer's task.
+         *
+         */
+        void taskLoop();
+
+        /**
+         * @brief The function that will be applied to each string in the buffer when it is removed.
+         *
+         */
+        std::function<void(std::string)> bufferFunc;
+
+        std::deque<std::string> buffer = {};
+
+        pros::Mutex mutex;
+        pros::Task task;
+
+        uint32_t rate;
+};
+} // namespace lemlib

CLOUDS/include/lemlib/logger/infoSink.hpp

@@ -0,0 +1,39 @@
+#pragma once
+
+#include <deque>
+
+#include "pros/rtos.hpp"
+#include "lemlib/logger/message.hpp"
+#include "lemlib/logger/baseSink.hpp"
+
+namespace lemlib {
+/**
+ * @brief Sink for sending messages to the terminal.
+ *
+ * This is the primary way of interacting with LemLib's logging implementation. This sink is used for printing useful
+ * information to the user's terminal.
+ * <h3> Example Usage </h3>
+ * @code
+ * lemlib::infoSink()->setLowestLevel(lemlib::Level::INFO);
+ * lemlib::infoSink()->info("info: {}!", "my cool info here");
+ * // Specify the order or placeholders
+ * lemlib::infoSink()->debug("{1} {0}!","world", "hello");
+ * // Specify the precision of floating point numbers
+ * lemlib::infoSink()->warn("Thingy exceeded value: {:.2f}!", 93.1234);
+ * @endcode
+ */
+class InfoSink : public BaseSink {
+    public:
+        /**
+         * @brief Construct a new Info Sink object
+         */
+        InfoSink();
+    private:
+        /**
+         * @brief Log the given message
+         *
+         * @param message
+         */
+        void sendMessage(const Message& message) override;
+};
+} // namespace lemlib

CLOUDS/include/lemlib/logger/logger.hpp

@@ -0,0 +1,26 @@
+#pragma once
+
+#include <memory>
+#include <array>
+
+#define FMT_HEADER_ONLY
+#include "fmt/core.h"
+
+#include "lemlib/logger/baseSink.hpp"
+#include "lemlib/logger/infoSink.hpp"
+#include "lemlib/logger/telemetrySink.hpp"
+
+namespace lemlib {
+
+/**
+ * @brief Get the info sink.
+ * @return std::shared_ptr<InfoSink>
+ */
+std::shared_ptr<InfoSink> infoSink();
+
+/**
+ * @brief Get the telemetry sink.
+ * @return std::shared_ptr<TelemetrySink>
+ */
+std::shared_ptr<TelemetrySink> telemetrySink();
+} // namespace lemlib

CLOUDS/include/lemlib/logger/message.hpp

@@ -0,0 +1,34 @@
+#pragma once
+
+#include <string>
+
+namespace lemlib {
+/**
+ * @brief Level of the message
+ *
+ */
+enum class Level { INFO, DEBUG, WARN, ERROR, FATAL };
+
+/**
+ * @brief A loggable message
+ *
+ */
+struct Message {
+        /* The message */
+        std::string message;
+
+        /** The level of the message */
+        Level level;
+
+        /** The time the message was logged, in milliseconds */
+        uint32_t time;
+};
+
+/**
+ * @brief Format a level
+ *
+ * @param level
+ * @return std::string
+ */
+std::string format_as(Level level);
+} // namespace lemlib

CLOUDS/include/lemlib/logger/stdout.hpp

@@ -0,0 +1,34 @@
+#pragma once
+
+#define FMT_HEADER_ONLY
+#include "fmt/core.h"
+
+#include "lemlib/logger/buffer.hpp"
+
+namespace lemlib {
+/**
+ * @brief Buffered printing to Stout.
+ *
+ * LemLib uses a buffered wrapper around stdout in order to guarantee that messages are printed at a constant
+ * rate, no matter how many different threads are trying to use the logger. This is a concern  because not every type of
+ * connection to the brain has the same amount of bandwidth.
+ */
+class BufferedStdout : public Buffer {
+    public:
+        BufferedStdout();
+
+        /**
+         * @brief Print a string (thread-safe).
+         *
+         */
+        template <typename... T> void print(fmt::format_string<T...> format, T&&... args) {
+            pushToBuffer(fmt::format(format, std::forward<T>(args)...));
+        }
+};
+
+/**
+ * @brief Get the buffered stdout.
+ *
+ */
+BufferedStdout& bufferedStdout();
+} // namespace lemlib

CLOUDS/include/lemlib/logger/telemetrySink.hpp

@@ -0,0 +1,34 @@
+#pragma once
+
+#include "lemlib/logger/baseSink.hpp"
+
+namespace lemlib {
+/**
+ * @brief Sink for sending telemetry data.
+ *
+ * This is the primary way of interacting with the telemetry portion of LemLib's logging implementation. This sink is
+ * used for sending data that is not meant to be viewed by the user, but will still be used by something else, like a
+ * data visualization tool. Messages sent through this sink will not be cleared from the terminal and not be visible to
+ * the user.
+
+ * <h3> Example Usage </h3>
+ * @code
+ * lemlib::telemetrySink()->setLowestLevel(lemlib::Level::INFO);
+ * lemlib::telemetrySink()->info("{},{}", motor1.get_temperature(), motor2.get_temperature());
+ * @endcode
+ */
+class TelemetrySink : public BaseSink {
+    public:
+        /**
+         * @brief Construct a new Telemetry Sink object
+         */
+        TelemetrySink();
+    private:
+        /**
+         * @brief Log the given message
+         *
+         * @param message
+         */
+        void sendMessage(const Message& message) override;
+};
+} // namespace lemlib

CLOUDS/include/lemlib/pid.hpp

@@ -0,0 +1,125 @@
+/**
+ * @file include/lemlib/pid.hpp
+ * @author Lemlib Team
+ * @brief FAPID class header
+ * @version 0.4.5
+ * @date 2023-01-15
+ *
+ * @copyright Copyright (c) 2023
+ *
+ */
+#pragma once
+#include <string>
+#include "pros/rtos.hpp"
+
+namespace lemlib {
+/**
+ * @brief Feedforward, Acceleration, Proportional, Integral, Derivative PID controller
+ *
+ * The controller does not loop on its own. It must be called in a loop.
+ * For example: while(!controller.settled) { controller.update(input, output); }
+ *
+ */
+class FAPID {
+    public:
+        /**
+         * @brief Construct a new FAPID
+         *
+         * @param kF feedfoward gain, multiplied by target and added to output. Set 0 if disabled
+         * @param kA acceleration gain, limits the change in output. Set 0 if disabled
+         * @param kP proportional gain, multiplied by error and added to output
+         * @param kI integral gain, multiplied by total error and added to output
+         * @param kD derivative gain, multiplied by change in error and added to output
+         * @param name name of the FAPID. Used for logging
+         */
+        FAPID(float kF, float kA, float kP, float kI, float kD, std::string name);
+        /**
+         * @brief Set gains
+         *
+         * @param kF feedfoward gain, multiplied by target and added to output. Set 0 if disabled
+         * @param kA acceleration gain, limits the change in output. Set 0 if disabled
+         * @param kP proportional gain, multiplied by error and added to output
+         * @param kI integral gain, multiplied by total error and added to output
+         * @param kD derivative gain, multiplied by change in error and added to output
+         */
+        void setGains(float kF, float kA, float kP, float kI, float kD);
+        /**
+         * @brief Set the exit conditions
+         *
+         * @param largeError range where error is considered large
+         * @param smallError range where error is considered small
+         * @param largeTime time in milliseconds t
+         * @param smallTime
+         * @param maxTime
+         */
+        void setExit(float largeError, float smallError, int largeTime, int smallTime, int maxTime);
+        /**
+         * @brief Update the FAPID
+         *
+         * @param target the target value
+         * @param position the current value
+         * @param log whether to check the most recent terminal input for user input. Default is false because logging
+         * multiple PIDs could slow down the program.
+         * @return float - output
+         */
+        float update(float target, float position, bool log = false);
+        /**
+         * @brief Reset the FAPID
+         */
+        void reset();
+        /**
+         * @brief Check if the FAPID has settled
+         *
+         * If the exit conditions have not been set, this function will always return false
+         *
+         * @return true - the FAPID has settled
+         * @return false - the FAPID has not settled
+         */
+        bool settled();
+        /**
+         * @brief initialize the FAPID logging system
+         *
+         * if this function is called, std::cin will be used to interact with the FAPID
+         *
+         * the user can interact with the FAPID through the terminal
+         * the user can access gains and other variables with the following format:
+         * <name>.<variable> to get the value of the variable
+         * <name>.<variable>_<value> to set the value of the variable
+         * for example:
+         * pid.kP_0.5 will set the kP value to 0.5
+         * list of variables thats value can be set:
+         * kF, kA, kP, kI, kD
+         * list of variables that can be accessed:
+         * kF, kA, kP, kI, kD, totalError
+         * list of functions that can be called:
+         * reset()
+         */
+        static void init();
+    private:
+        float kF;
+        float kA;
+        float kP;
+        float kI;
+        float kD;
+
+        float largeError;
+        float smallError;
+        int largeTime = 0;
+        int smallTime = 0;
+        int maxTime = -1; // -1 means no max time set, run forever
+
+        int largeTimeCounter = 0;
+        int smallTimeCounter = 0;
+        int startTime = 0;
+
+        float prevError = 0;
+        float totalError = 0;
+        float prevOutput = 0;
+
+        void log();
+        std::string name;
+        static std::string input;
+        static pros::Task* logTask;
+        static pros::Mutex logMutex;
+};
+} // namespace lemlib

CLOUDS/include/lemlib/pose.hpp

@@ -0,0 +1,105 @@
+/**
+ * @file include/lemlib/pose.hpp
+ * @author LemLib Team
+ * @brief Pose class declarations
+ * @version 0.4.5
+ * @date 2023-01-23
+ *
+ * @copyright Copyright (c) 2023
+ *
+ */
+#pragma once
+
+#include <string>
+
+namespace lemlib {
+class Pose {
+    public:
+        /** @brief x value*/
+        float x;
+        /** @brief y value*/
+        float y;
+        /** @brief theta value*/
+        float theta;
+        /**
+         * @brief Create a new pose
+         *
+         * @param x component
+         * @param y component
+         * @param theta heading. Defaults to 0
+         */
+        Pose(float x, float y, float theta = 0);
+        /**
+         * @brief Add a pose to this pose
+         *
+         * @param other other pose
+         * @return Pose
+         */
+        Pose operator+(const Pose& other);
+        /**
+         * @brief Subtract a pose from this pose
+         *
+         * @param other other pose
+         * @return Pose
+         */
+        Pose operator-(const Pose& other);
+        /**
+         * @brief Multiply a pose by this pose
+         *
+         * @param other other pose
+         * @return Pose
+         */
+        float operator*(const Pose& other);
+        /**
+         * @brief Multiply a pose by a float
+         *
+         * @param other float
+         * @return Pose
+         */
+        Pose operator*(const float& other);
+        /**
+         * @brief Divide a pose by a float
+         *
+         * @param other float
+         * @return Pose
+         */
+        Pose operator/(const float& other);
+        /**
+         * @brief Linearly interpolate between two poses
+         *
+         * @param other the other pose
+         * @param t t value
+         * @return Pose
+         */
+        Pose lerp(Pose other, float t);
+        /**
+         * @brief Get the distance between two poses
+         *
+         * @param other the other pose
+         * @return float
+         */
+        float distance(Pose other);
+        /**
+         * @brief Get the angle between two poses
+         *
+         * @param other the other pose
+         * @return float in radians
+         */
+        float angle(Pose other);
+        /**
+         * @brief Rotate a pose by an angle
+         *
+         * @param angle angle in radians
+         * @return Pose
+         */
+        Pose rotate(float angle);
+};
+
+/**
+ * @brief Format a pose
+ *
+ * @param pose
+ * @return std::string
+ */
+std::string format_as(const Pose& pose);
+} // namespace lemlib

CLOUDS/include/lemlib/timer.hpp

@@ -0,0 +1,71 @@
+#pragma once
+
+#include <cstdint>
+
+namespace lemlib {
+class Timer {
+    public:
+        /**
+         * @brief Construct a new Timer
+         *
+         * @param time how long to wait, in milliseconds
+         */
+        Timer(uint32_t time);
+        /**
+         * @brief Get the amount of time the timer was set to
+         *
+         * @return uint32_t time, in milliseconds
+         */
+        uint32_t getTimeSet();
+        /**
+         * @brief Get the amount of time left on the timer
+         *
+         * @return uint32_t time in milliseconds
+         */
+        uint32_t getTimeLeft();
+        /**
+         * @brief Get the amount of time passed on the timer
+         *
+         * @return uint32_t time in milliseconds
+         */
+        uint32_t getTimePassed();
+        /**
+         * @brief Get whether the timer is done or not
+         *
+         * @return true the timer is done
+         * @return false the timer is not done
+         */
+        bool isDone();
+        /**
+         * @brief Set the amount of time the timer should count down. Resets the timer
+         *
+         * @param time time in milliseconds
+         */
+        void set(uint32_t time);
+        /**
+         * @brief reset the timer
+         *
+         */
+        void reset();
+        /**
+         * @brief pause the timer
+         *
+         */
+        void pause();
+        /**
+         * @brief resume the timer
+         *
+         */
+        void resume();
+        /**
+         * @brief wait
+         *
+         */
+        void waitUntilDone();
+    private:
+        uint32_t period;
+        uint32_t lastTime;
+        uint32_t timeWaited = 0;
+        bool paused = false;
+};
+} // namespace lemlib

CLOUDS/include/lemlib/util.hpp

@@ -0,0 +1,95 @@
+/**
+ * @file include/lemlib/util.hpp
+ * @author LemLib Team
+ * @brief Utility functions declarations
+ * @version 0.4.5
+ * @date 2023-01-15
+ *
+ * @copyright Copyright (c) 2023
+ *
+ */
+
+#pragma once
+
+#include <vector>
+#include <math.h>
+#include "lemlib/pose.hpp"
+
+namespace lemlib {
+/**
+ * @brief Slew rate limiter
+ *
+ * @param target target value
+ * @param current current value
+ * @param maxChange maximum change. No maximum if set to 0
+
+ * @return float - the limited value
+ */
+float slew(float target, float current, float maxChange);
+
+/**
+ * @brief Convert radians to degrees
+ *
+ * @param rad radians
+ * @return float degrees
+ */
+constexpr float radToDeg(float rad) { return rad * 180 / M_PI; }
+
+/**
+ * @brief Convert degrees to radians
+ *
+ * @param deg degrees
+ * @return float radians
+ */
+constexpr float degToRad(float deg) { return deg * M_PI / 180; }
+
+/**
+ * @brief Calculate the error between 2 angles. Useful when calculating the error between 2 headings
+ *
+ * @param angle1
+ * @param angle2
+ * @param radians true if angle is in radians, false if not. False by default
+ * @return float wrapped angle
+ */
+float angleError(float angle1, float angle2, bool radians = false);
+
+/**
+ * @brief Return the sign of a number
+ *
+ * @param x the number to get the sign of
+ * @return int - -1 if negative, 1 if positive
+ */
+template <typename T> constexpr T sgn(T value) { return value < 0 ? -1 : 1; }
+
+/**
+ * @brief Return the average of a vector of numbers
+ *
+ * @param values
+ * @return float
+ */
+float avg(std::vector<float> values);
+
+/**
+ * @brief Exponential moving average
+ *
+ * @param current current measurement
+ * @param previous previous output
+ * @param smooth smoothing factor (0-1). 1 means no smoothing, 0 means no change
+ * @return float - the smoothed output
+ */
+float ema(float current, float previous, float smooth);
+
+/**
+ * @brief Get the signed curvature of a circle that intersects the first pose and the second pose
+ *
+ * @note The circle will be tangent to the theta value of the first pose
+ * @note The curvature is signed. Positive curvature means the circle is going clockwise, negative means
+ * counter-clockwise
+ * @note Theta has to be in radians and in standard form. That means 0 is right and increases counter-clockwise
+ *
+ * @param pose the first pose
+ * @param other the second pose
+ * @return float curvature
+ */
+float getCurvature(Pose pose, Pose other);
+} // namespace lemlib

CLOUDS/project.pros

@@ -4,6 +4,43 @@
         "project_name": "CLOUDS",
         "target": "v5",
         "templates": {
+            "LemLib": {
+                "location": "C:\\Users\\cjans\\AppData\\Roaming\\PROS\\templates\\LemLib@0.5.0-rc1",
+                "metadata": {
+                    "origin": "local"
+                },
+                "name": "LemLib",
+                "py/object": "pros.conductor.templates.local_template.LocalTemplate",
+                "supported_kernels": "^3.8.0",
+                "system_files": [
+                    "include\\lemlib\\pose.hpp",
+                    "include\\lemlib\\logger\\baseSink.hpp",
+                    "include\\lemlib\\logger\\stdout.hpp",
+                    "include\\lemlib\\logger\\logger.hpp",
+                    "include\\fmt\\args.h",
+                    "static\\example.txt",
+                    "include\\lemlib\\pid.hpp",
+                    "firmware\\LemLib.a",
+                    "include\\lemlib\\logger\\message.hpp",
+                    "include\\lemlib\\timer.hpp",
+                    "include\\fmt\\core.h",
+                    "include\\lemlib\\chassis\\chassis.hpp",
+                    "firmware\\asset.mk",
+                    "include\\fmt\\format.h",
+                    "include\\lemlib\\logger\\telemetrySink.hpp",
+                    "include\\lemlib\\chassis\\trackingWheel.hpp",
+                    "include\\lemlib\\asset.hpp",
+                    "include\\lemlib\\logger\\buffer.hpp",
+                    "include\\lemlib\\chassis\\odom.hpp",
+                    "include\\fmt\\format-inl.h",
+                    "include\\lemlib\\api.hpp",
+                    "include\\lemlib\\logger\\infoSink.hpp",
+                    "include\\lemlib\\util.hpp"
+                ],
+                "target": "v5",
+                "user_files": [],
+                "version": "0.5.0-rc1"
+            },
             "kernel": {
                 "location": "C:\\Users\\cjans\\AppData\\Roaming\\PROS\\templates\\kernel@3.8.0",
                 "metadata": {

CLOUDS/static/example.txt

@@ -0,0 +1,32 @@
+0, 0, 100
+0.028, 2, 100
+0.103, 3.998, 96.584
+0.239, 5.993, 92.136
+0.455, 7.981, 87.462
+0.774, 9.955, 82.524
+1.248, 11.897, 77.273
+1.925, 13.777, 71.639
+2.918, 15.509, 65.529
+4.329, 16.914, 58.805
+6.099, 17.825, 55.537
+8.042, 18.292, 55.436
+10.02, 18.588, 55.565
+11.969, 19.025, 55.313
+13.764, 19.889, 56.711
+15.208, 21.258, 62.282
+16.235, 22.968, 71.497
+16.949, 24.834, 71.51
+17.427, 26.775, 65.38
+17.762, 28.747, 58.611
+17.988, 30.733, 50.949
+18.13, 32.728, 41.909
+18.207, 34.726, 30.28
+18.243, 36.91, 0
+18.243, 36.91, 0
+18.569, 56.907, 0
+endData
+209.9
+100
+200
+0, 0, 0, 34.317, 18.243, 2.593, 18.243, 36.91
+#PATH.JERRYIO-DATA {"appVersion":"0.4.0","format":"LemLib v0.4.x (inch, byte-voltage)","gc":{"robotWidth":11.811023622047244,"robotHeight":11.811023622047244,"robotIsHolonomic":false,"showRobot":false,"uol":2.54,"pointDensity":2,"controlMagnetDistance":0.7750015500031,"fieldImage":{"displayName":"VRC 2024 - Over Under","signature":"VRC 2024 - Over Under","origin":{"__type":"built-in"}}},"paths":[{"segments":[{"controls":[{"uid":"YvRxlirKBP","x":0,"y":0,"lock":false,"visible":true,"heading":0,"__type":"end-point"},{"uid":"hfScvgIaVo","x":0,"y":34.31729518855657,"lock":false,"visible":true,"__type":"control"},{"uid":"cN87evim1M","x":18.2428478543563,"y":2.592652795838755,"lock":false,"visible":true,"__type":"control"},{"uid":"nc60yAjqBR","x":18.2428478543563,"y":36.90994798439532,"lock":false,"visible":true,"heading":0,"__type":"end-point"}],"speedProfiles":[],"uid":"OMgd6R8fjU"}],"pc":{"speedLimit":{"minLimit":{"value":0,"label":"0"},"maxLimit":{"value":127,"label":"127"},"step":1,"from":20,"to":100},"bentRateApplicableRange":{"minLimit":{"value":0,"label":"0"},"maxLimit":{"value":4,"label":"4"},"step":0.01,"from":0,"to":2.69},"maxDecelerationRate":209.9},"name":"Path","uid":"XPpO96nwPH","lock":false,"visible":true}]}