✨Okapi units (#49, #62)

include/EZ-Template/drive/drive.hpp

@@ -12,6 +12,7 @@ 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/QLength.hpp"
 #include "pros/motors.h"
 
 using namespace ez;
@@ -414,7 +415,12 @@ class Drive {
   /**
    * The position of the right motor.
    */
-  int right_sensor();
+  double right_sensor();
+
+  /**
+   * The position of the right motor.
+   */
+  int raw_right_sensor();
 
   /**
    * The velocity of the right motor.
@@ -434,7 +440,12 @@ class Drive {
   /**
    * The position of the left motor.
    */
-  int left_sensor();
+  double left_sensor();
+
+  /**
+   * The position of the left motor.
+   */
+  int raw_left_sensor();
 
   /**
    * The velocity of the left motor.
@@ -507,7 +518,7 @@ class Drive {
    * \param toggle_heading
    *        toggle for heading correction
    */
-  void set_drive_pid(double target, int speed, bool slew_on = false, bool toggle_heading = true);
+  void set_drive_pid(okapi::QLength p_target, int speed, bool slew_on = false, bool toggle_heading = true);
 
   /**
    * Sets the robot to turn using PID.
@@ -554,6 +565,14 @@ class Drive {
    */
   void wait_until(double 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
+   */
+  void wait_until(okapi::QLength target);
+
   /**
    * Autonomous interference detection.  Returns true when interfered, and false when nothing happened.
    */
@@ -673,28 +692,28 @@ class Drive {
   /**
    * Sets minimum slew distance constants.
    *
-   * \param fw
-   *        minimum distance for forward drive pd
-   * \param bw
-   *        minimum distance for backwards drive pd
+   * \param fwd
+   *        minimum distance for forward drive pd, okapi unit
+   * \param rev
+   *        minimum distance for backwards drive pd, okapi unit
    */
-  void set_slew_distance(int fwd, int rev);
+  void set_slew_distance(okapi::QLength fwd, okapi::QLength rev);
 
   /**
    * Set's constants for drive 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.
+   *        Sets smalL_error. Timer will start when error is within this.  Okapi unit.
    * \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.
+   *        Sets big_error. Timer will start when error is within this. Okapi unit.
    * \param p_velocity_exit_time
    *        Sets velocity_exit_time.  Timer will start when velocity is 0.
    */
-  void set_drive_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_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);
 
   /**
    * Set's constants for turn exit conditions.
@@ -783,14 +802,18 @@ class Drive {
    */
   double slew_calculate(slew_ &input, double current);
 
-  bool using_inches = false;
-
  private:  // !Auton
   bool drive_toggle = true;
   bool print_toggle = true;
   int swing_min = 0;
   int turn_min = 0;
   bool practice_mode_is_on = false;
+
+  /**
+   * Private wait until for drive
+   */
+  void wait_until_drive(double target);
+
   /**
    * Sets the chassis to voltage.
    *

include/main.h

@@ -58,6 +58,7 @@
 // using namespace pros::literals;
 // using namespace okapi;
 // using namespace ez;
+using namespace okapi::literals;
 
 /**
  * Prototypes for the competition control tasks are redefined here to ensure

src/EZ-Template/drive/drive.cpp

@@ -141,24 +141,21 @@ Drive::Drive(std::vector<int> left_motor_ports, std::vector<int> right_motor_por
 
 void Drive::set_defaults() {
   // PID Constants
-  headingPID = {11, 0, 20, 0};
-  forward_drivePID = {0.45, 0, 5, 0};
-  backward_drivePID = {0.45, 0, 5, 0};
-  turnPID = {5, 0.003, 35, 15};
-  swingPID = {7, 0, 45, 0};
-  leftPID = {0.45, 0, 5, 0};
-  rightPID = {0.45, 0, 5, 0};
+  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_turn_min(30);
   set_swing_min(30);
 
   // Slew constants
   set_slew_min_power(80, 80);
-  set_slew_distance(7, 7);
+  set_slew_distance(7_in, 7_in);
 
   // Exit condition constants
-  set_turn_exit_condition(100, 3, 500, 7, 500, 500);
-  set_swing_exit_condition(100, 3, 500, 7, 500, 500);
-  set_drive_exit_condition(80, 50, 300, 150, 500, 500);
+  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);
 
   // Modify joystick curve on controller (defaults to disabled)
   toggle_modify_curve_with_controller(true);
@@ -189,31 +186,6 @@ double Drive::get_tick_per_inch() {
 
 void Drive::set_ratio(double ratio) { RATIO = ratio; }
 
-void Drive::set_drive_pid_constants(double p, double i, double d, double p_start_i) {
-  set_drive_forward_pid_constants(p, i, d, p_start_i);
-  set_drive_backwards_pid_constants(p, i, d, p_start_i);
-}
-
-void Drive::set_drive_forward_pid_constants(double p, double i, double d, double p_start_i) {
-  forward_drivePID.set_constants(p, i, d, p_start_i);
-}
-
-void Drive::set_drive_backwards_pid_constants(double p, double i, double d, double p_start_i) {
-  backward_drivePID.set_constants(p, i, d, p_start_i);
-}
-
-void Drive::set_turn_pid_constants(double p, double i, double d, double p_start_i) {
-  turnPID.set_constants(p, i, d, p_start_i);
-}
-
-void Drive::set_swing_pid_constants(double p, double i, double d, double p_start_i) {
-  swingPID.set_constants(p, i, d, p_start_i);
-}
-
-void Drive::set_heading_pid_constants(double p, double i, double d, double p_start_i) {
-  headingPID.set_constants(p, i, d, p_start_i);
-}
-
 void Drive::private_set_tank(int left, int right) {
   if (pros::millis() < 1500) return;
 
@@ -258,24 +230,26 @@ void Drive::reset_drive_sensor() {
   }
 }
 
-int Drive::right_sensor() {
+int Drive::raw_right_sensor() {
   if (is_tracker == DRIVE_ADI_ENCODER)
     return right_tracker.get_value();
   else if (is_tracker == DRIVE_ROTATION)
     return right_rotation.get_position();
   return right_motors.front().get_position();
 }
+double Drive::right_sensor() { return raw_right_sensor() / get_tick_per_inch(); }
 int Drive::right_velocity() { return right_motors.front().get_actual_velocity(); }
 double Drive::right_mA() { return right_motors.front().get_current_draw(); }
 bool Drive::right_over_current() { return right_motors.front().is_over_current(); }
 
-int Drive::left_sensor() {
+int Drive::raw_left_sensor() {
   if (is_tracker == DRIVE_ADI_ENCODER)
     return left_tracker.get_value();
   else if (is_tracker == DRIVE_ROTATION)
     return left_rotation.get_position();
   return left_motors.front().get_position();
 }
+double Drive::left_sensor() { return raw_left_sensor() / get_tick_per_inch(); }
 int Drive::left_velocity() { return left_motors.front().get_actual_velocity(); }
 double Drive::left_mA() { return left_motors.front().get_current_draw(); }
 bool Drive::left_over_current() { return left_motors.front().is_over_current(); }

src/EZ-Template/drive/exit_conditions.cpp

@@ -9,20 +9,18 @@ file, You can obtain one at http://mozilla.org/MPL/2.0/.
 
 using namespace ez;
 
-
-void Drive::set_drive_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){
-  leftPID.set_exit_condition(p_small_exit_time, p_small_error, p_big_exit_time, p_big_error, p_velocity_exit_time, p_mA_timeout);
-  rightPID.set_exit_condition(p_small_exit_time, p_small_error, p_big_exit_time, p_big_error, p_velocity_exit_time, p_mA_timeout);
+void Drive::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) {
+  leftPID.set_exit_condition(p_small_exit_time, p_small_error.convert(okapi::inch), p_big_exit_time, p_big_error.convert(okapi::inch), p_velocity_exit_time, p_mA_timeout);
+  rightPID.set_exit_condition(p_small_exit_time, p_small_error.convert(okapi::inch), p_big_exit_time, p_big_error.convert(okapi::inch), p_velocity_exit_time, p_mA_timeout);
 }
 
-void Drive::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 Drive::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) {
   turnPID.set_exit_condition(p_small_exit_time, p_small_error, p_big_exit_time, p_big_error, p_velocity_exit_time, p_mA_timeout);
 }
-void Drive::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 Drive::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) {
   swingPID.set_exit_condition(p_small_exit_time, p_small_error, p_big_exit_time, p_big_error, p_velocity_exit_time, p_mA_timeout);
 }
 
-
 // User wrapper for exit condition
 void Drive::wait_drive() {
   // Let the PID run at least 1 iteration
@@ -36,7 +34,7 @@ void Drive::wait_drive() {
       right_exit = right_exit != RUNNING ? right_exit : rightPID.exit_condition(right_motors[0]);
       pros::delay(util::DELAY_TIME);
     }
-    if (print_toggle) std::cout << "  Left: " << exit_to_string(left_exit) << " Exit.   Right: " << exit_to_string(right_exit) << " Exit.\n";
+    if (print_toggle) std::cout << "  Left: " << exit_to_string(left_exit) << " Exit, error: " << leftPID.error << ".   Right: " << exit_to_string(right_exit) << " Exit, error: " << rightPID.error << ".\n";
 
     if (left_exit == mA_EXIT || left_exit == VELOCITY_EXIT || right_exit == mA_EXIT || right_exit == VELOCITY_EXIT) {
       interfered = true;
@@ -50,7 +48,7 @@ void Drive::wait_drive() {
       turn_exit = turn_exit != RUNNING ? turn_exit : turnPID.exit_condition({left_motors[0], right_motors[0]});
       pros::delay(util::DELAY_TIME);
     }
-    if (print_toggle) std::cout << "  Turn: " << exit_to_string(turn_exit) << " Exit.\n";
+    if (print_toggle) std::cout << "  Turn: " << exit_to_string(turn_exit) << " Exit, error:" << turnPID.error << ".\n";
 
     if (turn_exit == mA_EXIT || turn_exit == VELOCITY_EXIT) {
       interfered = true;
@@ -65,21 +63,20 @@ void Drive::wait_drive() {
       swing_exit = swing_exit != RUNNING ? swing_exit : swingPID.exit_condition(sensor);
       pros::delay(util::DELAY_TIME);
     }
-    if (print_toggle) std::cout << "  Swing: " << exit_to_string(swing_exit) << " Exit.\n";
+    if (print_toggle) std::cout << "  Swing: " << exit_to_string(swing_exit) << " Exit, error:" << swingPID.error << ".\n";
 
     if (swing_exit == mA_EXIT || swing_exit == VELOCITY_EXIT) {
       interfered = true;
     }
   }
 }
 
-// Function to wait until a certain position is reached.  Wrapper for exit condition.
-void Drive::wait_until(double target) {
+void Drive::wait_until_drive(double target) {
   // If robot is driving...
   if (mode == DRIVE) {
     // Calculate error between current and target (target needs to be an in between position)
-    int l_tar = l_start + (target * TICK_PER_INCH);
-    int r_tar = r_start + (target * TICK_PER_INCH);
+    int l_tar = l_start + target;
+    int r_tar = r_start + target;
     int l_error = l_tar - left_sensor();
     int r_error = r_tar - right_sensor();
     int l_sgn = util::sgn(l_error);
@@ -116,6 +113,23 @@ void Drive::wait_until(double target) {
       pros::delay(util::DELAY_TIME);
     }
   }
+}
+
+void Drive::wait_until(okapi::QLength target) {
+  // If robot is driving...
+  if (mode == DRIVE) {
+    wait_until_drive(target.convert(okapi::inch));
+  } else {
+    printf("QLength not supported for turn or swing!\n");
+  }
+}
+
+// Function to wait until a certain position is reached.  Wrapper for exit condition.
+void Drive::wait_until(double target) {
+  // If robot is driving...
+  if (mode == DRIVE) {
+    wait_until_drive(target);
+  }
 
   // If robot is turning or swinging...
   else if (mode == TURN || mode == SWING) {

src/EZ-Template/drive/pid_tasks.cpp

@@ -35,13 +35,9 @@ void Drive::ez_auto_task() {
 // Drive PID task
 void Drive::drive_pid_task() {
   // Compute PID
-  if (!using_inches) {
-    leftPID.compute(left_sensor());
-    rightPID.compute(right_sensor());
-  } else {
-    leftPID.compute(left_sensor() / TICK_PER_INCH);
-    rightPID.compute(right_sensor() / TICK_PER_INCH);
-  }
+  leftPID.compute(left_sensor());
+  rightPID.compute(right_sensor());
+
   headingPID.compute(get_gyro());
 
   // Compute slew