Added okapi time and angle units (#49)

include/EZ-Template/drive/drive.hpp

@@ -12,7 +12,9 @@ file, You can obtain one at http://mozilla.org/MPL/2.0/.
 
 #include "EZ-Template/PID.hpp"
 #include "EZ-Template/util.hpp"
+#include "okapi/api/units/QAngle.hpp"
 #include "okapi/api/units/QLength.hpp"
+#include "okapi/api/units/QTime.hpp"
 #include "pros/motors.h"
 
 using namespace ez;
@@ -509,8 +511,8 @@ class Drive {
   /**
    * Sets the robot to move forward using PID.
    *
-   * \param target
-   *        target value in inches
+   * \param p_target
+   *        target value in okapi length units
    * \param speed
    *        0 to 127, max speed during motion
    * \param slew_on
@@ -523,63 +525,75 @@ class Drive {
   /**
    * Sets the robot to turn using PID.
    *
-   * \param target
-   *        target value in degrees
+   * \param p_target
+   *        target value in okapi angle units
    * \param speed
    *        0 to 127, max speed during motion
    */
-  void set_turn_pid(double target, int speed);
+  void set_turn_pid(okapi::QAngle p_target, int speed);
 
   /**
-  * Sets the robot to turn relative to current heading using PID.
-  * 
-  * \param target
-  *        target in degrees relative to current heading
-  * \param speed
-  *        0 to 127, max speed during motion
-  */
-  void set_relative_turn_pid(double target, int speed);
+   * Sets the robot to turn relative to current heading using PID.
+   *
+   * \param p_target
+   *        target value in okapi angle units
+   * \param speed
+   *        0 to 127, max speed during motion
+   */
+  void set_relative_turn_pid(okapi::QAngle p_target, int speed);
 
   /**
    * Turn using only the left or right side.
    *
    * \param type
    *        L_SWING or R_SWING
-   * \param target
-   *        target value in degrees
+   * \param p_target
+   *        target value in okapi angle units
    * \param speed
    *        0 to 127, max speed during motion
    */
-  void set_swing_pid(e_swing type, double target, int speed);
+  void set_swing_pid(e_swing type, okapi::QAngle p_target, int speed);
+
+  /**
+   * Sets the robot to turn only using the left or right side relative to current heading using PID.
+   *
+   * \param type
+   *        L_SWING or R_SWING
+   * \param p_target
+   *        target value in okapi angle units
+   * \param speed
+   *        0 to 127, max speed during motion
+   */
+  void set_relative_swing_pid(e_swing type, okapi::QAngle p_target, int speed);
 
   /**
    * Resets all PID targets to 0.
    */
   void reset_pid_targets();
 
   /**
-   * Sets heading of gyro and target of PID.
+   * Sets heading of gyro and target of PID using okapi units.
    */
-  void set_angle(double angle);
+  void set_angle(okapi::QAngle p_target);
 
   /**
    * Lock the code in a while loop until the robot has settled.
    */
   void wait_drive();
 
   /**
-   * Lock the code in a while loop until this position has passed.
+   * Lock the code in a while loop until this position has passed for turning or swinging with okapi units.
    *
    * \param target
-   *        when driving, this is inches.  when turning, this is degrees.
+   *        for turns or swings, using okapi angle units
    */
-  void wait_until(double target);
+  void wait_until(okapi::QAngle target);
 
   /**
    * Lock the code in a while loop until this position has passed for driving with okapi units.
    *
    * \param target
-   *        for driving, using okapi units
+   *        for driving, using okapi length units
    */
   void wait_until(okapi::QLength target);
 
@@ -713,49 +727,49 @@ class Drive {
    * Set's constants for drive exit conditions.
    *
    * \param p_small_exit_time
-   *        Sets small_exit_time.  Timer for to exit within smalL_error.
+   *        Sets small_exit_time.  Timer for to exit within smalL_error. In okapi time units.
    * \param p_small_error
-   *        Sets smalL_error. Timer will start when error is within this.  Okapi unit.
+   *        Sets smalL_error. Timer will start when error is within this.  In okapi length units.
    * \param p_big_exit_time
-   *        Sets big_exit_time.  Timer for to exit within big_error.
+   *        Sets big_exit_time.  Timer for to exit within big_error.  In okapi time units.
    * \param p_big_error
-   *        Sets big_error. Timer will start when error is within this. Okapi unit.
+   *        Sets big_error. Timer will start when error is within this. In okapi length units.
    * \param p_velocity_exit_time
-   *        Sets velocity_exit_time.  Timer will start when velocity is 0.
+   *        Sets velocity_exit_time.  Timer will start when velocity is 0.  In okapi time units.
    */
-  void set_drive_exit_condition(int p_small_exit_time, okapi::QLength p_small_error, int p_big_exit_time, okapi::QLength p_big_error, int p_velocity_exit_time, int p_mA_timeout);
+  void set_drive_exit_condition(okapi::QTime p_small_exit_time, okapi::QLength p_small_error, okapi::QTime p_big_exit_time, okapi::QLength p_big_error, okapi::QTime p_velocity_exit_time, okapi::QTime p_mA_timeout);
 
   /**
    * Set's constants for turn exit conditions.
    *
    * \param p_small_exit_time
-   *        Sets small_exit_time.  Timer for to exit within smalL_error.
+   *        Sets small_exit_time.  Timer for to exit within smalL_error.  In okapi time units.
    * \param p_small_error
-   *        Sets smalL_error. Timer will start when error is within this.
+   *        Sets smalL_error. Timer will start when error is within this.  In okapi angle units.
    * \param p_big_exit_time
-   *        Sets big_exit_time.  Timer for to exit within big_error.
+   *        Sets big_exit_time.  Timer for to exit within big_error.  In okapi time units.
    * \param p_big_error
-   *        Sets big_error. Timer will start when error is within this.
+   *        Sets big_error. Timer will start when error is within this.  In okapi angle units.
    * \param p_velocity_exit_time
-   *        Sets velocity_exit_time.  Timer will start when velocity is 0.
+   *        Sets velocity_exit_time.  Timer will start when velocity is 0.  In okapi time units.
    */
-  void set_turn_exit_condition(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_turn_exit_condition(okapi::QTime p_small_exit_time, okapi::QAngle p_small_error, okapi::QTime p_big_exit_time, okapi::QAngle p_big_error, okapi::QTime p_velocity_exit_time, okapi::QTime p_mA_timeout);
 
   /**
    * Set's constants for swing exit conditions.
    *
    * \param p_small_exit_time
-   *        Sets small_exit_time.  Timer for to exit within smalL_error.
+   *        Sets small_exit_time.  Timer for to exit within smalL_error.  In okapi time units.
    * \param p_small_error
-   *        Sets smalL_error. Timer will start when error is within this.
+   *        Sets smalL_error. Timer will start when error is within this.  In okapi angle units.
    * \param p_big_exit_time
-   *        Sets big_exit_time.  Timer for to exit within big_error.
+   *        Sets big_exit_time.  Timer for to exit within big_error.  In okapi time units.
    * \param p_big_error
-   *        Sets big_error. Timer will start when error is within this.
+   *        Sets big_error. Timer will start when error is within this.  In okapi angle units.
    * \param p_velocity_exit_time
-   *        Sets velocity_exit_time.  Timer will start when velocity is 0.
+   *        Sets velocity_exit_time.  Timer will start when velocity is 0.  In okapi time units.
    */
-  void set_swing_exit_condition(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_swing_exit_condition(okapi::QTime p_small_exit_time, okapi::QAngle p_small_error, okapi::QTime p_big_exit_time, okapi::QAngle p_big_error, okapi::QTime p_velocity_exit_time, okapi::QTime p_mA_timeout);
 
   /**
    * Returns current tick_per_inch()
@@ -824,6 +838,11 @@ class Drive {
    */
   void wait_until_drive(double target);
 
+  /**
+   * Private wait until for drive
+   */
+  void wait_until_turn_swing(double target);
+
   /**
    * Sets the chassis to voltage.
    *

include/autons.hpp

@@ -10,7 +10,5 @@ void drive_and_turn();
 void wait_until_change_speed();
 void swing_example();
 void combining_movements();
-void interfered_example();
 
 void default_constants();
-void using_inches_constants();

src/EZ-Template/drive/drive.cpp

@@ -141,21 +141,21 @@ Drive::Drive(std::vector<int> left_motor_ports, std::vector<int> right_motor_por
 
 void Drive::set_defaults() {
   // PID Constants
-  set_heading_pid_constants(3, 0, 20, 0);
-  set_drive_pid_constants(15, 0, 150);
-  set_turn_pid_constants(3, 0, 20, 0);
-  set_swing_pid_constants(5, 0, 30, 0);
+  set_heading_pid_constants(3, 0, 0, 0);
+  set_drive_pid_constants(5, 0, 0, 0);
+  set_turn_pid_constants(1, 0, 22, 0);
+  set_swing_pid_constants(1, 0, 2, 0);
   set_turn_min(30);
   set_swing_min(30);
 
   // Slew constants
-  set_slew_min_power(80, 80);
-  set_slew_distance(7_in, 7_in);
+  set_slew_min_power(50, 50);
+  set_slew_distance(1_ft, 1_ft);
 
   // Exit condition constants
-  set_turn_exit_condition(200, 3, 500, 7, 750, 750);
-  set_swing_exit_condition(200, 3, 500, 7, 750, 750);
-  set_drive_exit_condition(200, 1_in, 500, 3_in, 750, 750);
+  set_turn_exit_condition(300_ms, 4_deg, 900_ms, 7_deg, 1.5_s, 1.5_s);
+  set_swing_exit_condition(300_ms, 4_deg, 900_ms, 7_deg, 1.5_s, 1.5_s);
+  set_drive_exit_condition(300_ms, 1_in, 900_ms, 3_in, 1.5_s, 1.5_s);
 
   // Modify joystick curve on controller (defaults to disabled)
   toggle_modify_curve_with_controller(true);