New exit conditions implemented into chassis (#33)

Still needs wiki page for #33.

Fixed issue with #12
Fixed issue with #35

docs/Docs/auton_functions.md

@@ -202,36 +202,6 @@ void autonomous() {
 ---
 
 
-## exit_condition()
-Iterative bool that checks if the robot has settled.  This is used in `wait_drive()` and `wait_until()`.   
-`targets` is a tuple of PID objects.  
-`exitConditions` is either `swing_exit`, `drive_exit`, or `turn_exit`.  
-`wait_until` is a bool that changes the printfs.  
-**Prototype**
-```cpp
-bool exit_condition(std::tuple<double, std::optional<double>> targets, exit_condition_ exitConditions, bool wait_until = false);
-```
-
-**Example**
-```cpp
-void autonomous() {
-  chassis.set_drive_pid(24, DRIVE_SPEED, true);
-  while (chassis.exit_condition(tuple{chassis.leftPID.get_target(), chassis.rightPID.get_target()}, chassis.drive_exit)) {
-    printf("Left Error: %f   Right Error: %f\n", chassis.leftPID.error, chassis.rightPID.error);
-    pros::delay(ez::util::DELAY_TIME);
-  }
-
-  chassis.set_turn_pid(90, TURN_SPEED);
-  while (chassis.exit_condition(tuple{chassis.swingPID.get_target(), std::nullopt}, chassis.swing_exit)) {
-    printf("Turn Error: %f", chassis.turnPID.error);
-    pros::delay(util::DELAY_TIME);
-  }
-}
-```
-
-
----
-
 
 ## set_max_speed()
 Sets the max speed of the drive. 

include/EZ-Template/PID.hpp

@@ -11,12 +11,39 @@ file, You can obtain one at http://mozilla.org/MPL/2.0/.
 
 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
+   */
+  PID(double p, double i, double d, double start_i = 0);
+  void set_constants(double p, double i, double d, 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;
@@ -26,9 +53,6 @@ class PID {
     int mA_timeout = 0;
   };
 
-  PID();
-  PID(double p, double i, double d, double start_i = 0);
-  void set_constants(double p, double i, double d, double p_start_i = 0);
   /**
    * Set's constants for exit conditions.
    *
@@ -45,27 +69,90 @@ class PID {
    */
   void set_exit_condition(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);
 
-  int i = 0, j = 0, k = 0, l = 0;
-  bool is_mA = false;
-  void reset_timers();
-
+  /**
+   * Set's target.
+   *
+   * \param target
+   *        Target for PID.
+   */
   void set_target(double input);
+
+  /**
+   * Computes PID.
+   *
+   * \param current
+   *        Current sensor library.
+   */
   double compute(double current);
+
+  /**
+   * Returns target value.
+   */
   double get_target();
+
+  /**
+   * Returns constants.
+   */
   Constants get_constants();
+
+  /**
+   * Resets all variables to 0.  This does not reset constants.
+   */
   void reset_variables();
+
+  /**
+   * Constants
+   */
   Constants constants;
+
+  /**
+   * Exit
+   */
   exit_condition_ exit;
 
-  ez::exit_output exit_condition(std::vector<pros::Motor>, bool print = false);
+  /**
+   * 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);
+
+  /**
+   * 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 reset_timers();
 };

include/EZ-Template/drive/drive.hpp

@@ -534,33 +534,6 @@ class Drive {
    */
   void set_slew_distance(int fwd, int rev);
 
-  /**
-   * Exit condition struct.
-   */
-  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;
-  };
-
-  /**
-   * Exit condition for turning.
-   */
-  exit_condition_ turn_exit;
-
-  /**
-   * Exit condition for swinging.
-   */
-  exit_condition_ swing_exit;
-
-  /**
-   * Exit condition for driving.
-   */
-  exit_condition_ drive_exit;
-
   /**
    * Set's constants for exit conditions.
    *
@@ -577,19 +550,22 @@ class Drive {
    * \param p_velocity_exit_time
    *        Sets velocity_exit_time.  Timer will start when velocity is 0.
    */
-  void set_exit_condition(exit_condition_ &type, int p_small_exit_time, double p_small_error, int p_big_exit_time, double p_big_error, int p_velocity_exit_time, int p_mA_timeout);
+  void set_exit_condition(int type, int p_small_exit_time, double p_small_error, int p_big_exit_time, double p_big_error, int p_velocity_exit_time, int p_mA_timeout);
 
   /**
-   * Iterative exit condition. 
-   *
-   * \param targets
-   *        leftPID, rightPID, turnPID or swingPID
-   * \param exitConditions
-   *        turn_exit, swing_exit, or drive_exit
-   * \param wait_until = false
-   *        changes print statements
+   * Exit condition for turning.
+   */
+  const int turn_exit = 1;
+
+  /**
+   * Exit condition for swinging.
+   */
+  const int swing_exit = 2;
+
+  /**
+   * Exit condition for driving.
    */
-  bool exit_condition(std::tuple<double, std::optional<double>> targets, exit_condition_ exitConditions, bool wait_until = false);
+  const int drive_exit = 3;
 
  private:  // !Auton
 

include/EZ-Template/util.hpp

@@ -69,21 +69,29 @@ enum exit_output { RUNNING = 1,
                    VELOCITY_EXIT = 4,
                    mA_EXIT = 5 };
 
+/**
+ * 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 is_reversed(double input);
+
 /**
  * Returns input restricted to min-max threshold
  */
 double clip_num(double input, double min, double max);
+
 /**
  * Is the SD card plugged in?
  */

src/EZ-Template/PID.cpp

@@ -82,16 +82,18 @@ void PID::reset_timers() {
 
 exit_output PID::exit_condition(bool print) {
   // If the robot gets within the target, make sure it's there for small_timeout amount of time
-  if (abs(error) < exit.small_error) {
-    j += util::DELAY_TIME;
-    i = 0;  // While this is running, don't run big thresh
-    if (j > exit.small_exit_time) {
-      reset_timers();
-      if (print) printf(" Small Exit\n");
-      return SMALL_EXIT;
+  if (exit.small_error != 0) {
+    if (abs(error) < exit.small_error) {
+      j += util::DELAY_TIME;
+      i = 0;  // While this is running, don't run big thresh
+      if (j > exit.small_exit_time) {
+        reset_timers();
+        if (print) printf(" Small Exit\n");
+        return SMALL_EXIT;
+      }
+    } else {
+      j = 0;
     }
-  } else {
-    j = 0;
   }
 
   // If the robot is close to the target, start a timer.  If the robot doesn't get closer within
@@ -111,7 +113,7 @@ exit_output PID::exit_condition(bool print) {
 
   // If the motor velocity is 0,the code will timeout and set interfered to true.
   if (exit.velocity_exit_time != 0) {  // Check if this condition is enabled
-    if (derivative == 0) {
+    if (abs(derivative) <= 0.05) {
       k += util::DELAY_TIME;
       if (k > exit.velocity_exit_time) {
         reset_timers();
@@ -126,6 +128,24 @@ exit_output PID::exit_condition(bool print) {
   return RUNNING;
 }
 
+exit_output PID::exit_condition(pros::Motor sensor, bool print) {
+  // If the motors are pulling too many mA, the code will timeout and set interfered to true.
+  if (exit.mA_timeout != 0) {  // Check if this condition is enabled
+    if (sensor.is_over_current()) {
+      l += util::DELAY_TIME;
+      if (l > exit.mA_timeout) {
+        reset_timers();
+        if (print) printf(" mA Exit\n");
+        return mA_EXIT;
+      }
+    } else {
+      l = 0;
+    }
+  }
+
+  return exit_condition(print);
+}
+
 exit_output PID::exit_condition(std::vector<pros::Motor> sensor, bool print) {
   // If the motors are pulling too many mA, the code will timeout and set interfered to true.
   if (exit.mA_timeout != 0) {  // Check if this condition is enabled

src/EZ-Template/drive/drive.cpp

@@ -186,12 +186,10 @@ void Drive::set_tank(int left, int right) {
   if (pros::millis() < 1500) return;
 
   for (auto i : left_motors) {
-    if (pto_check(i)) break;  // If the motor is in the pto list, don't do anything to the motor.
-    i.move_voltage(left * (12000.0 / 127.0));
+    if (!pto_check(i)) i.move_voltage(left * (12000.0 / 127.0));  // If the motor is in the pto list, don't do anything to the motor.
   }
   for (auto i : right_motors) {
-    if (pto_check(i)) break;  // If the motor is in the pto list, don't do anything to the motor.
-    i.move_voltage(right * (12000.0 / 127.0));
+    if (!pto_check(i)) i.move_voltage(right * (12000.0 / 127.0));  // If the motor is in the pto list, don't do anything to the motor.
   }
 }
 
@@ -201,12 +199,10 @@ void Drive::set_drive_current_limit(int mA) {
   }
   CURRENT_MA = mA;
   for (auto i : left_motors) {
-    if (pto_check(i)) break;  // If the motor is in the pto list, don't do anything to the motor.
-    i.set_current_limit(abs(mA));
+    if (!pto_check(i)) i.set_current_limit(abs(mA));  // If the motor is in the pto list, don't do anything to the motor.
   }
   for (auto i : right_motors) {
-    if (pto_check(i)) break;  // If the motor is in the pto list, don't do anything to the motor.
-    i.set_current_limit(abs(mA));
+    if (!pto_check(i)) i.set_current_limit(abs(mA));  // If the motor is in the pto list, don't do anything to the motor.
   }
 }
 
@@ -306,12 +302,10 @@ bool Drive::imu_calibrate() {
 void Drive::set_drive_brake(pros::motor_brake_mode_e_t brake_type) {
   CURRENT_BRAKE = brake_type;
   for (auto i : left_motors) {
-    if (pto_check(i)) break;  // If the motor is in the pto list, don't do anything to the motor.
-    i.set_brake_mode(brake_type);
+    if (!pto_check(i)) i.set_brake_mode(brake_type);  // If the motor is in the pto list, don't do anything to the motor.
   }
   for (auto i : right_motors) {
-    if (pto_check(i)) break;  // If the motor is in the pto list, don't do anything to the motor.
-    i.set_brake_mode(brake_type);
+    if (!pto_check(i)) i.set_brake_mode(brake_type);  // If the motor is in the pto list, don't do anything to the motor.
   }
 }