334 lines
7.1 KiB
C++
334 lines
7.1 KiB
C++
/*
|
|
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
|