lemlib

RELENTLESS/include/lemlib/api.hpp

@@ -0,0 +1,17 @@
+/**
+ * @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"

RELENTLESS/include/lemlib/chassis/chassis.hpp

@@ -0,0 +1,173 @@
+/**
+ * @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 "pros/motors.hpp"
+#include "pros/imu.hpp"
+#include "lemlib/chassis/trackingWheel.hpp"
+#include "lemlib/pose.hpp"
+
+namespace lemlib {
+/**
+ * @brief Struct containing all the sensors used for odometry
+ *
+ * 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
+ */
+typedef struct {
+        TrackingWheel* vertical1;
+        TrackingWheel* vertical2;
+        TrackingWheel* horizontal1;
+        TrackingWheel* horizontal2;
+        pros::Imu* imu;
+} OdomSensors_t;
+
+/**
+ * @brief Struct containing constants for a chassis controller
+ *
+ * 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
+ */
+typedef struct {
+        float kP;
+        float kD;
+        float smallError;
+        float smallErrorTimeout;
+        float largeError;
+        float largeErrorTimeout;
+        float slew;
+} ChassisController_t;
+
+/**
+ * @brief Struct containing constants for a 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 wheels (2.75, 3.25, 4, 4.125)
+ * @param rpm the rpm of the wheels
+ */
+typedef struct {
+        pros::Motor_Group* leftMotors;
+        pros::Motor_Group* rightMotors;
+        float trackWidth;
+        float wheelDiameter;
+        float rpm;
+} Drivetrain_t;
+
+/**
+ * @brief Chassis class
+ *
+ */
+class Chassis {
+    public:
+        /**
+         * @brief Construct a new Chassis
+         *
+         * @param drivetrain drivetrain to be used for the chassis
+         * @param lateralSettings settings for the lateral controller
+         * @param angularSettings settings for the angular controller
+         * @param sensors sensors to be used for odometry
+         */
+        Chassis(Drivetrain_t drivetrain, ChassisController_t lateralSettings, ChassisController_t angularSettings,
+                OdomSensors_t sensors);
+        /**
+         * @brief Calibrate the chassis sensors
+         *
+         */
+        void calibrate();
+        /**
+         * @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(double x, double y, double 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 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 reversed whether the robot should turn in the opposite direction. false by default
+         * @param maxSpeed the maximum speed the robot can turn at. Default is 200
+         * @param log whether the chassis should log the turnTo function. false by default
+         */
+        void turnTo(float x, float y, int timeout, bool reversed = false, float maxSpeed = 127, bool log = false);
+        /**
+         * @brief Move the chassis towards the target point
+         *
+         * The PID logging ids are "angularPID" and "lateralPID"
+         *
+         * @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
+         * @param log whether the chassis should log the turnTo function. false by default
+         */
+        void moveTo(float x, float y, int timeout, float maxSpeed = 200, bool log = false);
+        /**
+         * @brief Move the chassis along a path
+         *
+         * @param filePath file path to the path. No need to preface it with /usd/
+         * @param timeout the maximum time the robot can spend moving
+         * @param lookahead the lookahead distance. Units in inches. Larger values will make the robot move faster but
+         * will follow the path less accurately
+         * @param reverse whether the robot should follow the path in reverse. false by default
+         * @param maxSpeed the maximum speed the robot can move at
+         * @param log whether the chassis should log the path on a log file. false by default.
+         */
+        void follow(const char* filePath, int timeout, float lookahead, bool reverse = false, float maxSpeed = 127,
+                    bool log = false);
+    private:
+        ChassisController_t lateralSettings;
+        ChassisController_t angularSettings;
+        Drivetrain_t drivetrain;
+        OdomSensors_t odomSensors;
+};
+} // namespace lemlib

RELENTLESS/include/lemlib/chassis/odom.hpp

@@ -0,0 +1,50 @@
+/**
+ * @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_t sensors, lemlib::Drivetrain_t 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 Update the pose of the robot
+ *
+ */
+void update();
+/**
+ * @brief Initialize the odometry system
+ *
+ */
+void init();
+} // namespace lemlib

RELENTLESS/include/lemlib/chassis/trackingWheel.hpp

@@ -0,0 +1,80 @@
+/**
+ * @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 {
+class TrackingWheel {
+    public:
+        /**
+         * @brief Create a new tracking wheel
+         *
+         * @param encoder the optical shaft encoder to use
+         * @param diameter diameter of the tracking wheel in inches
+         * @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 diameter, float distance, float gearRatio = 1);
+        /**
+         * @brief Create a new tracking wheel
+         *
+         * @param encoder the v5 rotation sensor to use
+         * @param diameter diameter of the tracking wheel in inches
+         * @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 diameter, float distance, float gearRatio = 1);
+        /**
+         * @brief Create a new tracking wheel
+         *
+         * @param motors the motor group to use
+         * @param diameter diameter of the drivetrain wheels in inches
+         * @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 diameter, 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

RELENTLESS/include/lemlib/logger.hpp

@@ -0,0 +1,139 @@
+/**
+ * @file include/lemlib/logger.hpp
+ * @author LemLib Team
+ * @brief A Logger for LemLib.
+ * @version 0.4.5
+ * @date 2023-02-12
+ *
+ * @copyright Copyright (c) 2023
+ *
+ */
+
+#pragma once
+
+namespace lemlib {
+
+static bool debug = false;
+static bool verbose = false;
+
+namespace logger {
+
+/**
+ * @brief A level enumeration.
+ *
+ * Debug: Only enabled if lemlib::logger::debug is true
+ * Info: General information
+ * Warn: Warnings, usually not critical/doesn't affect the robot
+ * Error: Errors, usually critical and affects the robot
+ * Fatal: Fatal errors, crashes the program
+ *
+ * @note The log level is inclusive. For example, if the log level is set to
+ */
+enum class Level { DEBUG, INFO, WARN, ERROR, FATAL };
+
+static Level lowestLevel = Level::INFO;
+
+/**
+ * @brief Whether or not to log debug messages.
+ *
+ * @return true if debug is enabled
+ */
+bool isDebug();
+/**
+ * @brief Sets lemlib::debug
+ *
+ * @param debug the new value
+ */
+void setDebug(bool debug);
+
+/**
+ * @brief Whether or not to log info messages.
+ *
+ * If false, only log messages with a level of lemlib::logger::Level::WARN
+ * or higher will be logged
+ */
+bool isVerbose();
+/**
+ * @brief Sets lemlib::verbose
+ *
+ * @param verbose the new value
+ */
+void setVerbose(bool verbose);
+
+/**
+ * @brief The current lowest log level.
+ *
+ * @return the lowest loggable level
+ */
+Level getLowestLevel();
+
+/**
+ * @brief Sets the lowest loggable level
+ *
+ * @param level the new lowest loggable level
+ */
+void setLowestLevel(Level level);
+
+/**
+ * @brief Logs a message with an exception
+ *
+ * @param level the level of the message
+ * @param message the message
+ * @param exception the exception
+ */
+void log(Level level, const char* message, const char* exception);
+/**
+ * @brief Logs a message
+ *
+ * @param level the level of the message
+ * @param message the message
+ */
+void log(Level level, const char* message);
+
+/**
+ * @brief Logs a debug message
+ *
+ * @param message
+ */
+void debug(const char* message);
+/**
+ * @brief Logs an info message
+ *
+ * @param message
+ */
+void info(const char* message);
+/**
+ * @brief Logs a warning message
+ *
+ * @param message
+ */
+void warn(const char* message);
+/**
+ * @brief Logs an error message
+ *
+ * @param message
+ * @param exception
+ */
+void error(const char* message, const char* exception);
+/**
+ * @brief Logs an error message
+ *
+ * @param message
+ */
+void error(const char* message);
+/**
+ * @brief Logs a fatal message
+ *
+ * @param message
+ * @param exception
+ */
+void fatal(const char* message, const char* exception);
+/**
+ * @brief Logs a fatal message
+ *
+ * @param message
+ */
+void fatal(const char* message);
+
+} // namespace logger
+} // namespace lemlib

RELENTLESS/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

RELENTLESS/include/lemlib/pose.hpp

@@ -0,0 +1,95 @@
+/**
+ * @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
+
+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);
+};
+} // namespace lemlib

RELENTLESS/include/lemlib/util.hpp

@@ -0,0 +1,77 @@
+/**
+ * @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>
+
+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
+ */
+float radToDeg(float rad);
+
+/**
+ * @brief Convert degrees to radians
+ *
+ * @param deg degrees
+ * @return float radians
+ */
+float degToRad(float deg);
+
+/**
+ * @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 float - -1 if negative, 1 if positive
+ */
+float sgn(float x);
+
+/**
+ * @brief Return the average of a vector of numbers
+ *
+ * @param values
+ * @return float
+ */
+float avg(std::vector<float> values);
+
+/**
+ * @brief Return the average of a vector of numbers
+ *
+ * @param values
+ * @return double
+ */
+double avg(std::vector<double> values);
+} // namespace lemlib

RELENTLESS/project.pros

@@ -4,6 +4,29 @@
         "project_name": "RELENTLESS",
         "target": "v5",
         "templates": {
+            "LemLib": {
+                "location": "C:\\Users\\cjans\\AppData\\Roaming\\PROS\\templates\\LemLib@0.4.7",
+                "metadata": {
+                    "origin": "local"
+                },
+                "name": "LemLib",
+                "py/object": "pros.conductor.templates.local_template.LocalTemplate",
+                "supported_kernels": "^3.8.0",
+                "system_files": [
+                    "firmware\\LemLib.a",
+                    "include\\lemlib\\chassis\\odom.hpp",
+                    "include\\lemlib\\pose.hpp",
+                    "include\\lemlib\\util.hpp",
+                    "include\\lemlib\\chassis\\trackingWheel.hpp",
+                    "include\\lemlib\\pid.hpp",
+                    "include\\lemlib\\api.hpp",
+                    "include\\lemlib\\logger.hpp",
+                    "include\\lemlib\\chassis\\chassis.hpp"
+                ],
+                "target": "v5",
+                "user_files": [],
+                "version": "0.4.7"
+            },
             "kernel": {
                 "location": "C:\\Users\\cjans\\AppData\\Roaming\\PROS\\templates\\kernel@3.8.0",
                 "metadata": {

RELENTLESS/src/main.cpp

@@ -1,4 +1,5 @@
 #include "main.h"
+#include "lemlib/api.hpp"
 
 /**
  * A callback function for LLEMU's center button.