add lemlib mod

This commit is contained in:
ary 2023-12-01 08:32:37 -05:00
parent 1387d2a289
commit 540a12c11c
24 changed files with 9954 additions and 0 deletions

BIN
CLOUDS/LemLib@0.5.0-rc1.zip Normal file

Binary file not shown.

10
CLOUDS/firmware/asset.mk Normal file
View File

@ -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 $^ $@

207
CLOUDS/include/fmt/args.h Normal file
View File

@ -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_

2666
CLOUDS/include/fmt/core.h Normal file

File diff suppressed because it is too large Load Diff

File diff suppressed because it is too large Load Diff

4113
CLOUDS/include/fmt/format.h Normal file

File diff suppressed because it is too large Load Diff

View File

@ -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"

View File

@ -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}; \
}

View File

@ -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

View File

@ -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

View File

@ -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

View File

@ -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

View File

@ -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

View File

@ -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

View File

@ -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

View File

@ -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

View File

@ -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

View File

@ -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

View File

@ -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

View File

@ -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

View File

@ -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

View File

@ -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

View File

@ -4,6 +4,43 @@
"project_name": "CLOUDS", "project_name": "CLOUDS",
"target": "v5", "target": "v5",
"templates": { "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": { "kernel": {
"location": "C:\\Users\\cjans\\AppData\\Roaming\\PROS\\templates\\kernel@3.8.0", "location": "C:\\Users\\cjans\\AppData\\Roaming\\PROS\\templates\\kernel@3.8.0",
"metadata": { "metadata": {

32
CLOUDS/static/example.txt Normal file
View File

@ -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}]}