Uploading the first files

include/EZ-Template/util.hpp

@@ -0,0 +1,333 @@
+/*
+This Source Code Form is subject to the terms of the Mozilla Public
+License, v. 2.0. If a copy of the MPL was not distributed with this
+file, You can obtain one at http://mozilla.org/MPL/2.0/.
+*/
+
+#pragma once
+
+#include <bits/stdc++.h>
+#include <stdio.h>
+#include <string.h>
+
+#include "api.h"
+#include "okapi/api/units/QAngle.hpp"
+#include "okapi/api/units/QLength.hpp"
+#include "okapi/api/units/QTime.hpp"
+#include "util.hpp"
+
+using namespace okapi::literals;
+
+/**
+ * Controller.
+ */
+extern pros::Controller master;
+
+namespace ez {
+
+/**
+ * Prints our branding all over your pros terminal.
+ */
+void ez_template_print();
+
+/**
+ * Prints to the brain screen in one string.
+ *
+ * Splits input between lines with '\n' or when text longer then 32 characters.
+ *
+ * \param text
+ *        input string
+ * \param line
+ *        starting line to print on, defaults to 0
+ */
+void screen_print(std::string text, int line = 0);
+
+/////
+//
+// Public Variables
+//
+/////
+
+/**
+ * Enum for split and single stick arcade.
+ */
+enum e_type { SINGLE = 0,
+              SPLIT = 1 };
+
+/**
+ * Enum for split and single stick arcade.
+ */
+enum e_swing { LEFT_SWING = 0,
+               RIGHT_SWING = 1 };
+
+/**
+ * Enum for PID::exit_condition outputs.
+ */
+enum exit_output { RUNNING = 1,
+                   SMALL_EXIT = 2,
+                   BIG_EXIT = 3,
+                   VELOCITY_EXIT = 4,
+                   mA_EXIT = 5,
+                   ERROR_NO_CONSTANTS = 6 };
+
+/**
+ * Enum for split and single stick arcade.
+ */
+enum e_mode { DISABLE = 0,
+              SWING = 1,
+              TURN = 2,
+              TURN_TO_POINT = 3,
+              DRIVE = 4,
+              POINT_TO_POINT = 5,
+              PURE_PURSUIT = 6 };
+
+/**
+ * Enum for drive directions.
+ */
+enum drive_directions { FWD = 0,
+                        FORWARD = FWD,
+                        fwd = FWD,
+                        forward = FWD,
+                        REV = 1,
+                        REVERSE = REV,
+                        rev = REV,
+                        reverse = REV };
+
+/**
+ * Enum for turn types.
+ */
+enum e_angle_behavior { raw = 0,
+                        left_turn = 1,
+                        LEFT_TURN = 1,
+                        counterclockwise = 1,
+                        ccw = 1,
+                        right_turn = 2,
+                        RIGHT_TURN = 2,
+                        clockwise = 2,
+                        cw = 2,
+                        shortest = 3,
+                        longest = 4 };
+
+const double ANGLE_NOT_SET = 0.0000000000000000000001;
+const okapi::QAngle p_ANGLE_NOT_SET = 0.0000000000000000000001_deg;
+
+/**
+ * Struct for coordinates.
+ */
+typedef struct pose {
+  double x;
+  double y;
+  double theta = ANGLE_NOT_SET;
+} pose;
+
+/**
+ * Struct for united coordinates.
+ */
+typedef struct united_pose {
+  okapi::QLength x;
+  okapi::QLength y;
+  okapi::QAngle theta = p_ANGLE_NOT_SET;
+} united_pose;
+
+/**
+ * Struct for odom movements.
+ */
+typedef struct odom {
+  pose target;
+  drive_directions drive_direction;
+  int max_xy_speed;
+  e_angle_behavior turn_behavior = shortest;
+} odom;
+
+/**
+ * Struct for united odom movements.
+ */
+typedef struct united_odom {
+  united_pose target;
+  drive_directions drive_direction;
+  int max_xy_speed;
+  e_angle_behavior turn_behavior = shortest;
+} united_odom;
+
+/**
+ * Outputs string for exit_condition enum.
+ */
+std::string exit_to_string(exit_output input);
+
+namespace util {
+extern bool AUTON_RAN;
+
+/**
+ * Returns the amount of places after a decimal, maxing out at 6.
+ *
+ * \param input
+ *        your input value with decimals
+ * \param min
+ *        minimum number of decimal places to print, this is defaulted to 0
+ */
+int places_after_decimal(double input, int min = 0);
+
+/**
+ * Returns a string with a specific number of decimal points.
+ *
+ * \param input
+ *        your input value
+ * \param n
+ *        the amount of decimals you want to display
+ */
+std::string to_string_with_precision(double input, int n = 2);
+
+/**
+ * Returns 1 if input is positive and -1 if input is negative.
+ *
+ * \param input
+ *        your input value
+ */
+int sgn(double input);
+
+/**
+ * Returns true if the input is < 0.
+ *
+ * \param input
+ *        your input value
+ */
+bool reversed_active(double input);
+
+/**
+ * Returns input restricted to min-max threshold.
+ *
+ * \param input
+ *        your input value
+ * \param max
+ *        the maximum input can be
+ * \param min
+ *        the minimum input can be
+ */
+double clamp(double input, double max, double min);
+
+/**
+ * Returns input restricted to min-max threshold.
+ *
+ * The minimum used is negative max.
+ *
+ * \param input
+ *        your input value
+ * \param max
+ *        the absolute value maximum input can be
+ */
+double clamp(double input, double max);
+
+/**
+ * Is the SD card plugged in?
+ */
+const bool SD_CARD_ACTIVE = pros::usd::is_installed();
+
+/**
+ * Delay time for tasks, this is set to 10 ms.
+ */
+const int DELAY_TIME = 10;
+
+/**
+ * Converts radians to degrees.
+ *
+ * \param input
+ *        your input radian
+ */
+double to_deg(double input);
+
+/**
+ * Converts degrees to radians.
+ *
+ * \param input
+ *        your input degree
+ */
+double to_rad(double input);
+
+/**
+ * Returns the angle between two points.
+ *
+ * \param itarget
+ *        target position
+ * \param icurrent
+ *        current position
+ */
+double absolute_angle_to_point(pose itarget, pose icurrent);
+
+/**
+ * Returns the distance between two points.
+ *
+ * \param itarget
+ *        target position
+ * \param icurrent
+ *        current position
+ */
+double distance_to_point(pose itarget, pose icurrent);
+
+/**
+ * Constrains an angle between 180 and -180.
+ *
+ * \param theta
+ *        input angle in degrees
+ */
+double wrap_angle(double theta);
+
+/**
+ * Returns a new pose that is projected off of the current pose.
+ *
+ * \param added
+ *        how far to project a new point
+ * \param icurrent
+ *        point to project off of
+ */
+pose vector_off_point(double added, pose icurrent);
+
+/**
+ * Returns the shortest angle for the robot to turn to in order to get to target.
+ *
+ * \param target
+ *        target value in degrees
+ * \param current
+ *        current value in degrees
+ * \param print = false
+ *        will print the new value if this is true, defaults to false
+ */
+double turn_shortest(double target, double current, bool print = false);
+
+/**
+ * Returns the farthest away angle for the robot to turn to in order to get to target.
+ *
+ * \param target
+ *        target value in degrees
+ * \param current
+ *        current value in degrees
+ * \param print = false
+ *        will print the new value if this is true, defaults to false
+ */
+double turn_longest(double target, double current, bool print = false);
+
+/**
+ * Converts pose with okapi units to a pose without okapi units.
+ *
+ * \param input
+ *        a pose with units
+ */
+pose united_pose_to_pose(united_pose input);
+
+/**
+ * Converts vector of poses with okapi units to a vector of poses without okapi units.
+ *
+ * \param inputs
+ *        poses with units
+ */
+std::vector<odom> united_odoms_to_odoms(std::vector<united_odom> inputs);
+
+/**
+ * Converts odom movement with united pose to an odom movement without united pose.
+ *
+ * \param input
+ *        odom movement with units
+ */
+odom united_odom_to_odom(united_odom input);
+
+}  // namespace util
+}  // namespace ez