Refactored everything.

include/EZ-Template/PID.hpp

@@ -0,0 +1,205 @@
+/*
+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 "EZ-Template/util.hpp"
+#include "api.h"
+
+namespace ez {
+class PID {
+ public:
+  /**
+   * Default constructor.
+   */
+  PID();
+
+  /**
+   * Constructor with constants.
+   *
+   * \param p
+   *        kP
+   * \param i
+   *        ki
+   * \param d
+   *        kD
+   * \param p_start_i
+   *        error value that i starts within
+   * \param name
+   *        std::string of name that prints
+   */
+  PID(double p, double i = 0, double d = 0, double start_i = 0, std::string name = "");
+
+  /**
+   * Set constants for PID.
+   *
+   * \param p
+   *        kP
+   * \param i
+   *        ki
+   * \param d
+   *        kD
+   * \param p_start_i
+   *        error value that i starts within
+   */
+  void constants_set(double p, double i = 0, double d = 0, double p_start_i = 0);
+
+  /**
+   * Struct for constants.
+   */
+  struct Constants {
+    double kp;
+    double ki;
+    double kd;
+    double start_i;
+  };
+
+  /**
+   * Struct for exit condition.
+   */
+  struct exit_condition_ {
+    int small_exit_time = 0;
+    double small_error = 0;
+    int big_exit_time = 0;
+    double big_error = 0;
+    int velocity_exit_time = 0;
+    int mA_timeout = 0;
+  };
+
+  /**
+   * Set's constants for exit conditions.
+   *
+   * \param p_small_exit_time
+   *        Sets small_exit_time.  Timer for to exit within smalL_error.
+   * \param p_small_error
+   *        Sets smalL_error. Timer will start when error is within this.
+   * \param p_big_exit_time
+   *        Sets big_exit_time.  Timer for to exit within big_error.
+   * \param p_big_error
+   *        Sets big_error. Timer will start when error is within this.
+   * \param p_velocity_exit_time
+   *        Sets velocity_exit_time.  Timer will start when velocity is 0.
+   */
+  void exit_condition_set(int p_small_exit_time, double p_small_error, int p_big_exit_time = 0, double p_big_error = 0, int p_velocity_exit_time = 0, int p_mA_timeout = 0);
+
+  /**
+   * Sets target.
+   *
+   * \param target
+   *        Target for PID.
+   */
+  void target_set(double input);
+
+  /**
+   * Computes PID.
+   *
+   * \param current
+   *        Current sensor library.
+   */
+  double compute(double current);
+
+  /**
+   * Returns target value.
+   */
+  double target_get();
+
+  /**
+   * Returns constants.
+   */
+  Constants constants_get();
+
+  /**
+   * Resets all variables to 0.  This does not reset constants.
+   */
+  void variables_reset();
+
+  /**
+   * Constants
+   */
+  Constants constants;
+
+  /**
+   * Exit
+   */
+  exit_condition_ exit;
+
+  /**
+   * Iterative exit condition for PID.
+   *
+   * \param print = false
+   *        if true, prints when complete.
+   */
+  ez::exit_output exit_condition(bool print = false);
+
+  /**
+   * Iterative exit condition for PID.
+   *
+   * \param sensor
+   *        A pros motor on your mechanism.
+   * \param print = false
+   *        if true, prints when complete.
+   */
+  ez::exit_output exit_condition(pros::Motor sensor, bool print = false);
+
+  /**
+   * Iterative exit condition for PID.
+   *
+   * \param sensor
+   *        Pros motors on your mechanism.
+   * \param print = false
+   *        if true, prints when complete.
+   */
+  ez::exit_output exit_condition(std::vector<pros::Motor> sensor, bool print = false);
+
+  /**
+   * Sets the name of the PID that prints during exit conditions.
+   *
+   * \param name
+   *        a string that is the name you want to print
+   */
+  void name_set(std::string name);
+
+  /**
+   * Returns the name of the PID that prints during exit conditions.
+   */
+  std::string name_get();
+
+  /**
+   * Enables / disables i resetting when sgn of error changes.  True resets, false doesn't.
+   *
+   * \param toggle
+   *        true resets, false doesn't
+   */
+  void i_reset_toggle(bool toggle);
+
+  /**
+   * Returns if i will reset when sgn of error changes.  True resets, false doesn't.
+   */
+  bool i_reset_get();
+
+  /**
+   * PID variables.
+   */
+  double output;
+  double cur;
+  double error;
+  double target;
+  double prev_error;
+  double integral;
+  double derivative;
+  long time;
+  long prev_time;
+
+ private:
+  int i = 0, j = 0, k = 0, l = 0;
+  bool is_mA = false;
+  void timers_reset();
+  std::string name;
+  bool name_active = false;
+  void exit_condition_print(ez::exit_output exit_type);
+  bool reset_i_sgn = true;
+};
+};  // namespace ez

include/EZ-Template/api.hpp

@@ -0,0 +1,16 @@
+/*
+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 "EZ-Template/PID.hpp"
+#include "EZ-Template/auton.hpp"
+#include "EZ-Template/auton_selector.hpp"
+#include "EZ-Template/drive/drive.hpp"
+#include "EZ-Template/piston.hpp"
+#include "EZ-Template/sdcard.hpp"
+#include "EZ-Template/slew.hpp"
+#include "EZ-Template/util.hpp"

include/EZ-Template/auton.hpp

@@ -0,0 +1,21 @@
+/*
+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 <functional>
+#include <iostream>
+
+namespace ez {
+class Auton {
+ public:
+  Auton();
+  Auton(std::string, std::function<void()>);
+  std::string Name;
+  std::function<void()> auton_call;
+
+ private:
+};
+}  // namespace ez

include/EZ-Template/auton_selector.hpp

@@ -0,0 +1,26 @@
+/*
+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 <tuple>
+
+#include "EZ-Template/auton.hpp"
+
+using namespace std;
+
+namespace ez {
+class AutonSelector {
+ public:
+  std::vector<Auton> Autons;
+  int auton_page_current;
+  int auton_count;
+  AutonSelector();
+  AutonSelector(std::vector<Auton> autons);
+  void selected_auton_call();
+  void selected_auton_print();
+  void autons_add(std::vector<Auton> autons);
+};
+}  // namespace ez

include/EZ-Template/piston.hpp

@@ -0,0 +1,75 @@
+/*
+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 "api.h"
+
+namespace ez {
+class Piston {
+ public:
+  /**
+   * Piston used throughout.
+   */
+  pros::ADIDigitalOut piston;
+
+  /**
+   * Piston constructor.  This class keeps track of piston state.  The starting position of your piston is FALSE.
+   *
+   * \param input_port
+   *        The ports of your pistons.
+   * \param default_state
+   *        Starting state of your piston.
+   */
+  Piston(int input_port, bool default_state = false);
+
+  /**
+   * Piston constructor in 3 wire expander.  The starting position of your piston is FALSE.
+   *
+   * \param input_ports
+   *        The ports of your pistons.
+   * \param default_state
+   *        Starting state of your piston.
+   */
+  Piston(int input_port, int expander_smart_port, bool default_state = false);
+
+  /**
+   * Sets the piston to the input.
+   *
+   * \param input
+   *        True or false.  True sets to the opposite of the starting position.
+   */
+  void set(bool input);
+
+  /**
+   * Returns current piston state.
+   */
+  bool get();
+
+  /**
+   * One button toggle for the piston.
+   *
+   * \param toggle
+   *        An input button.
+   */
+  void button_toggle(int toggle);
+
+  /**
+   * Two buttons trigger the piston.  Active is enabled, deactive is disabled.
+   *
+   * \param active
+   *        Sets piston to true.
+   * \param active
+   *        Sets piston to false.
+   */
+  void buttons(int active, int deactive);
+
+ private:
+  bool reversed = false;
+  bool current = false;
+  int last_press = 0;
+};
+};  // namespace ez

include/EZ-Template/sdcard.hpp

@@ -0,0 +1,71 @@
+/*
+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 "EZ-Template/auton_selector.hpp"
+#include "api.h"
+
+namespace ez {
+namespace as {
+extern AutonSelector auton_selector;
+/**
+ * Sets sd card to current page.
+ */
+void auton_selector_initialize();
+
+/**
+ * Sets the sd card to current page.
+ */
+void auto_sd_update();
+
+/**
+ * Increases the page by 1.
+ */
+void page_up();
+
+/**
+ * Decreases the page by 1.
+ */
+void page_down();
+
+/**
+ * Initializes LLEMU and sets up callbacks for auton selector.
+ */
+void initialize();
+
+/**
+ * Wrapper for pros::lcd::shutdown.
+ */
+void shutdown();
+
+/**
+ * Returns true if the auton selector is running
+ */
+bool enabled();
+
+inline bool auton_selector_running;
+
+extern bool turn_off;
+
+extern pros::ADIDigitalIn* limit_switch_left;
+extern pros::ADIDigitalIn* limit_switch_right;
+/**
+ * Initialize two limitswithces to change pages on the lcd
+ *
+ * @param left_limit_port
+ *        port for the left limit switch
+ * @param right_limit_port
+ *        port for the right limit switch
+ */
+void limit_switch_lcd_initialize(pros::ADIDigitalIn* right_limit, pros::ADIDigitalIn* left_limit = nullptr);
+
+/**
+ * pre_auto_task
+ */
+void limitSwitchTask();
+}  // namespace as
+}  // namespace ez

include/EZ-Template/slew.hpp

@@ -0,0 +1,89 @@
+/*
+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 "EZ-Template/util.hpp"
+#include "api.h"
+
+namespace ez {
+class slew {
+ public:
+  slew();
+
+  /**
+   * Struct for constants.
+   */
+  struct Constants {
+    double min_speed = 0;
+    double distance_to_travel = 0;
+  };
+  Constants constants;
+
+  /**
+   * Sets constants for slew.  Slew ramps up the speed of the robot until the set distance is traveled.
+   *
+   * \param distance
+   *        the distance the robot travels before reaching max speed
+   * \param minimum_speed
+   *        the starting speed for the movement
+   */
+  slew(double distance, int minimum_speed);
+
+  /**
+   * Sets constants for slew.  Slew ramps up the speed of the robot until the set distance is traveled.
+   *
+   * \param distance
+   *        the distance the robot travels before reaching max speed
+   * \param minimum_speed
+   *        the starting speed for the movement
+   */
+  void constants_set(double distance, int minimum_speed);
+  Constants constants_get();
+
+  /**
+   * Initializes slew for the motion.
+   *
+   * \param enabled
+   *        true enables slew, false disables slew
+   * \param maximum_speed
+   *        the target speed the robot will ramp up too
+   * \param target
+   *        the target position for the motion
+   * \param current
+   *        the position at the start of the motion
+   */
+  void initialize(bool enabled, double maximum_speed, double target, double current);
+
+  /**
+   * Iterates slew and ramps up speed the farther along the motion the robot gets.
+   *
+   * \param current
+   *        current sensor value
+   */
+  double iterate(double current);
+
+  /**
+   * Returns true if slew is enabled, and false if it isn't.
+   */
+  bool enabled();
+
+  /**
+   * Returns the last output of iterate.
+   */
+  double output();
+
+ private:
+  int sign = 0;
+  double error = 0;
+  double x_intercept = 0;
+  double y_intercept = 0;
+  double slope = 0;
+  double last_output = 0;
+  bool is_enabled = false;
+  double max_speed = 0;
+};
+};  // namespace ez

include/EZ-Template/util.hpp

@@ -0,0 +1,107 @@
+/*
+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"
+
+/**
+ * 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.  Use '\n' for a new line
+ * @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,
+              DRIVE = 3 };
+
+/**
+ * Outputs string for exit_condition enum.
+ */
+std::string exit_to_string(exit_output input);
+
+namespace util {
+extern bool AUTON_RAN;
+
+/**
+ * Returns 1 if input is positive and -1 if input is negative
+ */
+int sgn(double input);
+
+/**
+ * Returns true if the input is < 0
+ */
+bool reversed_active(double input);
+
+/**
+ * Returns input restricted to min-max threshold
+ */
+double clamp(double input, double max, double min);
+
+/**
+ * Is the SD card plugged in?
+ */
+const bool SD_CARD_ACTIVE = pros::usd::is_installed();
+
+/**
+ * Delay time for tasks
+ */
+const int DELAY_TIME = 10;
+}  // namespace util
+}  // namespace ez