Formatting

include/EZ-Template/drive/drive.hpp

@@ -1412,10 +1412,10 @@ class Drive {
   void drive_imu_display_loading(int iter);
 
   /**
-   * Get angle of the robot, depending on focused sensor
+   * Get angle of the robot, depending on focused sensor.
    */
-  double drive_get_angle();
-  
+  double drive_angle_get();
+
   /**
    * Practice mode for driver practice that shuts off the drive if you go max speed.
    *
@@ -3592,7 +3592,7 @@ class Drive {
   float p_increment = 0.1, i_increment = 0.001, d_increment = 0.25, start_i_increment = 1.0;
 
   /**
-   * @brief 
+   * @brief
    * Get the scaled imu value from given imu
    */
   double get_this_imu(pros::Imu* imu);
@@ -3656,10 +3656,6 @@ class Drive {
   double l_start = 0;
   double r_start = 0;
 
-  /**
-   * Current position of imu
-   */
-
   /**
    * Enable/disable modifying controller curve with controller.
    */

include/EZ-Template/util.hpp

@@ -227,11 +227,6 @@ const bool SD_CARD_ACTIVE = pros::usd::is_installed();
  */
 const int DELAY_TIME = 10;
 
-/**
- * Thresold for disconnect time
- */
-const int DISCONNECT_THRESHOLD = 10;
-
 /**
  * Converts radians to degrees.
  *

src/EZ-Template/drive/drive.cpp

@@ -266,10 +266,9 @@ void Drive::drive_defaults_set() {
   as::limit_switch_lcd_initialize(nullptr, nullptr);
 }
 
-double Drive::drive_get_angle()
-{
+double Drive::drive_angle_get() {
   /*if there is a good imu*/
-  if(imu != nullptr) return drive_imu_get();
+  if (imu != nullptr) return drive_imu_get();
 
   return INT_MAX;
 }

src/EZ-Template/drive/exit_conditions.cpp

@@ -253,10 +253,10 @@ void Drive::wait_until_turn_swing(double target) {
   }
 
   // Create new target that is the shortest from current
-  target = new_turn_target_compute(target, drive_get_angle(), shortest);
+  target = new_turn_target_compute(target, drive_angle_get(), shortest);
 
   // Calculate error between current and target (target needs to be an in between position)
-  double g_error = target - drive_get_angle();
+  double g_error = target - drive_angle_get();
   int g_sgn = util::sgn(g_error);
 
   exit_output turn_exit = RUNNING;
@@ -265,7 +265,7 @@ void Drive::wait_until_turn_swing(double target) {
   pros::Motor& sensor = current_swing == ez::LEFT_SWING ? left_motors[0] : right_motors[0];
 
   while (true) {
-    g_error = target - drive_get_angle();
+    g_error = target - drive_angle_get();
 
     // If turning...
     if (mode == TURN || mode == TURN_TO_POINT) {
@@ -276,7 +276,7 @@ void Drive::wait_until_turn_swing(double target) {
           turn_exit = turn_exit != RUNNING ? turn_exit : turnPID.exit_condition({left_motors[0], right_motors[0]});
           pros::delay(util::DELAY_TIME);
         } else {
-          if (print_toggle) std::cout << "  Turn: " << exit_to_string(turn_exit) << " Wait Until Exit Failsafe, triggered at " << drive_get_angle() << " instead of " << target << "\n";
+          if (print_toggle) std::cout << "  Turn: " << exit_to_string(turn_exit) << " Wait Until Exit Failsafe, triggered at " << drive_angle_get() << " instead of " << target << "\n";
 
           if (turn_exit == mA_EXIT || turn_exit == VELOCITY_EXIT) {
             interfered = true;
@@ -286,7 +286,7 @@ void Drive::wait_until_turn_swing(double target) {
       }
       // Once we've past target, return
       else if (util::sgn(g_error) != g_sgn) {
-        if (print_toggle) printf("  Turn Wait Until Exit Success, triggered at %.2f.  Target: %.2f\n", drive_get_angle(), target);
+        if (print_toggle) printf("  Turn Wait Until Exit Success, triggered at %.2f.  Target: %.2f\n", drive_angle_get(), target);
         turnPID.timers_reset();
         return;
       }
@@ -301,7 +301,7 @@ void Drive::wait_until_turn_swing(double target) {
           swing_exit = swing_exit != RUNNING ? swing_exit : swingPID.exit_condition(sensor);
           pros::delay(util::DELAY_TIME);
         } else {
-          if (print_toggle) std::cout << "  Swing: " << exit_to_string(swing_exit) << " Wait Until Exit Failsafe, triggered at " << drive_get_angle() << " instead of " << target << "\n";
+          if (print_toggle) std::cout << "  Swing: " << exit_to_string(swing_exit) << " Wait Until Exit Failsafe, triggered at " << drive_angle_get() << " instead of " << target << "\n";
 
           if (swing_exit == mA_EXIT || swing_exit == VELOCITY_EXIT) {
             interfered = true;
@@ -311,7 +311,7 @@ void Drive::wait_until_turn_swing(double target) {
       }
       // Once we've past target, return
       else if (util::sgn(g_error) != g_sgn) {
-        if (print_toggle) printf("  Swing Wait Until Exit Success, triggered at %.2f. Target: %.2f\n", drive_get_angle(), target);
+        if (print_toggle) printf("  Swing Wait Until Exit Success, triggered at %.2f. Target: %.2f\n", drive_angle_get(), target);
         swingPID.timers_reset();
         return;
       }

src/EZ-Template/drive/maintenance.cpp

@@ -13,15 +13,14 @@ using namespace ez;
 void Drive::check_imu_task() {
   // Don't let this function run if IMU calibration is incomplete
   if (!imu_calibration_complete) return;
-  
-  //erase indices only if imu val equals previous one
+
+  // erase indices only if imu val equals previous one
   good_imus.erase(std::remove_if(good_imus.begin(), good_imus.end(), [this](pros::Imu *n) { return n->get_status() == pros::ImuStatus::error || errno == PROS_ERR || get_this_imu(n) == prev_imu_values[n->get_port()]; }), good_imus.end());
-
-  for (size_t i = 0; i < good_imus.size(); i++)
-  {
+
+  for (size_t i = 0; i < good_imus.size(); i++) {
     prev_imu_values[good_imus[i]->get_port()] = get_this_imu(good_imus[i]);
   }
-  
+
   double imu_val = drive_imu_scaler_get();
   if ((imu->get_status() == pros::ImuStatus::error || errno == PROS_ERR || prev_imu_value == imu_val) && !good_imus.empty())  // not sure if errno is needed yet. I think it is??
   {

src/EZ-Template/drive/pid_tasks.cpp

@@ -51,7 +51,7 @@ void Drive::drive_pid_task() {
   leftPID.compute(drive_sensor_left());
   rightPID.compute(drive_sensor_right());
 
-  headingPID.compute(drive_get_angle());
+  headingPID.compute(drive_angle_get());
 
   // Compute slew
   slew_left.iterate(drive_sensor_left());
@@ -93,7 +93,7 @@ void Drive::drive_pid_task() {
 void Drive::turn_pid_task() {
   // Compute PID if it's a normal turn
   if (mode == TURN) {
-    turnPID.compute(drive_get_angle());
+    turnPID.compute(drive_angle_get());
   }
   // Compute PID if we're turning to point
   else {
@@ -104,7 +104,7 @@ void Drive::turn_pid_task() {
   }
 
   // Compute slew
-  slew_turn.iterate(drive_get_angle());
+  slew_turn.iterate(drive_angle_get());
 
   // Clip gyroPID to max speed
   double gyro_out = util::clamp(turnPID.output, slew_turn.output(), -slew_turn.output());
@@ -123,12 +123,12 @@ void Drive::turn_pid_task() {
 // Swing PID task
 void Drive::swing_pid_task() {
   // Compute PID
-  swingPID.compute(drive_get_angle());
+  swingPID.compute(drive_angle_get());
   leftPID.compute(drive_sensor_left());
   rightPID.compute(drive_sensor_right());
 
   // Compute slew
-  double current = slew_swing_using_angle ? drive_get_angle() : (current_swing == LEFT_SWING ? drive_sensor_left() : drive_sensor_right());
+  double current = slew_swing_using_angle ? drive_angle_get() : (current_swing == LEFT_SWING ? drive_sensor_left() : drive_sensor_right());
   slew_swing.iterate(current);
 
   // Clip swingPID to max speed

src/EZ-Template/drive/set_pid/set_odom_pid.cpp

@@ -402,7 +402,7 @@ void Drive::raw_pid_odom_ptp_set(odom imovement, bool slew_on) {
   point_to_face = find_point_to_face(odom_pose_get(), {imovement.target.x, imovement.target.y}, current_drive_direction, true);
   double target = util::absolute_angle_to_point(point_to_face[!ptf1_running], odom_pose_get());  // Calculate the point for angle to face
   if (imovement.turn_behavior != raw) {
-    odom_imu_start = drive_imu_get();
+    odom_imu_start = drive_angle_get();
     current_angle_behavior = imovement.turn_behavior;
   }
 

src/EZ-Template/drive/set_pid/set_swing_pid.cpp

@@ -93,7 +93,7 @@ ez::e_angle_behavior Drive::pid_swing_behavior_get() { return default_swing_type
 /////
 // Absolute
 void Drive::pid_swing_set(e_swing type, double target, int speed) {
-  bool slew_on = is_swing_slew_enabled(type, target, drive_imu_get());
+  bool slew_on = is_swing_slew_enabled(type, target, drive_angle_get());
   pid_swing_set(type, target, speed, 0, pid_swing_behavior_get(), slew_on);
 }
 void Drive::pid_swing_set(e_swing type, okapi::QAngle p_target, int speed) {
@@ -104,7 +104,7 @@ void Drive::pid_swing_set(e_swing type, okapi::QAngle p_target, int speed) {
 void Drive::pid_swing_relative_set(e_swing type, double target, int speed) {
   // Figure out if going forward or backward
   double absolute_heading = target + headingPID.target_get();
-  bool slew_on = is_swing_slew_enabled(type, absolute_heading, drive_imu_get());
+  bool slew_on = is_swing_slew_enabled(type, absolute_heading, drive_angle_get());
   pid_swing_relative_set(type, target, speed, 0, pid_swing_behavior_get(), slew_on);
 }
 void Drive::pid_swing_relative_set(e_swing type, okapi::QAngle p_target, int speed) {
@@ -117,7 +117,7 @@ void Drive::pid_swing_relative_set(e_swing type, okapi::QAngle p_target, int spe
 /////
 // Absolute
 void Drive::pid_swing_set(e_swing type, double target, int speed, e_angle_behavior behavior) {
-  bool slew_on = is_swing_slew_enabled(type, target, drive_imu_get());
+  bool slew_on = is_swing_slew_enabled(type, target, drive_angle_get());
   pid_swing_set(type, target, speed, 0, behavior, slew_on);
 }
 void Drive::pid_swing_set(e_swing type, okapi::QAngle p_target, int speed, e_angle_behavior behavior) {
@@ -128,7 +128,7 @@ void Drive::pid_swing_set(e_swing type, okapi::QAngle p_target, int speed, e_ang
 void Drive::pid_swing_relative_set(e_swing type, double target, int speed, e_angle_behavior behavior) {
   // Figure out if going forward or backward
   double absolute_heading = target + headingPID.target_get();
-  bool slew_on = is_swing_slew_enabled(type, absolute_heading, drive_imu_get());
+  bool slew_on = is_swing_slew_enabled(type, absolute_heading, drive_angle_get());
   pid_swing_relative_set(type, target, speed, 0, behavior, slew_on);
 }
 void Drive::pid_swing_relative_set(e_swing type, okapi::QAngle p_target, int speed, e_angle_behavior behavior) {
@@ -141,18 +141,18 @@ void Drive::pid_swing_relative_set(e_swing type, okapi::QAngle p_target, int spe
 /////
 // Absolute
 void Drive::pid_swing_set(e_swing type, double target, int speed, int opposite_speed) {
-  bool slew_on = is_swing_slew_enabled(type, target, drive_imu_get());
+  bool slew_on = is_swing_slew_enabled(type, target, drive_angle_get());
   pid_swing_set(type, target, speed, opposite_speed, pid_swing_behavior_get(), slew_on);
 }
 void Drive::pid_swing_set(e_swing type, okapi::QAngle p_target, int speed, int opposite_speed) {
   double target = p_target.convert(okapi::degree);  // Convert okapi unit to degree
-  bool slew_on = is_swing_slew_enabled(type, target, drive_imu_get());
+  bool slew_on = is_swing_slew_enabled(type, target, drive_angle_get());
   pid_swing_set(type, target, speed, opposite_speed, slew_on);
 }
 // Relative
 void Drive::pid_swing_relative_set(e_swing type, double target, int speed, int opposite_speed) {
   double absolute_heading = target + headingPID.target_get();
-  bool slew_on = is_swing_slew_enabled(type, absolute_heading, drive_imu_get());
+  bool slew_on = is_swing_slew_enabled(type, absolute_heading, drive_angle_get());
   pid_swing_relative_set(type, target, speed, opposite_speed, pid_swing_behavior_get(), slew_on);
 }
 void Drive::pid_swing_relative_set(e_swing type, okapi::QAngle p_target, int speed, int opposite_speed) {
@@ -185,18 +185,18 @@ void Drive::pid_swing_relative_set(e_swing type, okapi::QAngle p_target, int spe
 /////
 // Absolute
 void Drive::pid_swing_set(e_swing type, double target, int speed, int opposite_speed, e_angle_behavior behavior) {
-  bool slew_on = is_swing_slew_enabled(type, target, drive_imu_get());
+  bool slew_on = is_swing_slew_enabled(type, target, drive_angle_get());
   pid_swing_set(type, target, speed, opposite_speed, behavior, slew_on);
 }
 void Drive::pid_swing_set(e_swing type, okapi::QAngle p_target, int speed, int opposite_speed, e_angle_behavior behavior) {
   double target = p_target.convert(okapi::degree);  // Convert okapi unit to degree
-  bool slew_on = is_swing_slew_enabled(type, target, drive_imu_get());
+  bool slew_on = is_swing_slew_enabled(type, target, drive_angle_get());
   pid_swing_set(type, target, speed, opposite_speed, behavior);
 }
 // Relative
 void Drive::pid_swing_relative_set(e_swing type, double target, int speed, int opposite_speed, e_angle_behavior behavior) {
   double absolute_heading = target + headingPID.target_get();
-  bool slew_on = is_swing_slew_enabled(type, absolute_heading, drive_imu_get());
+  bool slew_on = is_swing_slew_enabled(type, absolute_heading, drive_angle_get());
   pid_swing_relative_set(type, target, speed, opposite_speed, behavior, slew_on);
 }
 void Drive::pid_swing_relative_set(e_swing type, okapi::QAngle p_target, int speed, int opposite_speed, e_angle_behavior behavior) {
@@ -281,12 +281,12 @@ void Drive::pid_swing_set(e_swing type, double target, int speed, int opposite_s
 
   // Compute new turn target based on new angle
   target = flip_angle_target(target);
-  target = new_turn_target_compute(target, drive_imu_get(), current_angle_behavior);
+  target = new_turn_target_compute(target, drive_angle_get(), current_angle_behavior);
 
   // Print targets
   if (print_toggle) printf("Swing Started... Target Value: %.2f\n", target);
 
-  chain_sensor_start = drive_imu_get();
+  chain_sensor_start = drive_angle_get();
   chain_target_start = target;
   used_motion_chain_scale = 0.0;
 

src/EZ-Template/drive/set_pid/set_turn_pid.cpp

@@ -123,11 +123,11 @@ void Drive::pid_turn_set(double target, int speed, e_angle_behavior behavior, bo
 
   // Compute new turn target based on new angle
   target = flip_angle_target(target);
-  target = new_turn_target_compute(target, drive_imu_get(), current_angle_behavior);
+  target = new_turn_target_compute(target, drive_angle_get(), current_angle_behavior);
 
   // Print targets
   if (print_toggle) printf("Turn Started... Target Value: %.2f\n", target);
-  chain_sensor_start = drive_imu_get();
+  chain_sensor_start = drive_angle_get();
   chain_target_start = target;
   used_motion_chain_scale = 0.0;
 
@@ -176,7 +176,7 @@ void Drive::pid_turn_set(united_pose p_itarget, drive_directions dir, int speed,
 /////
 void Drive::pid_turn_set(pose itarget, drive_directions dir, int speed, e_angle_behavior behavior, bool slew_on) {
   itarget = flip_pose(itarget);
-  odom_imu_start = drive_imu_get();
+  odom_imu_start = drive_angle_get();
 
   current_drive_direction = dir;
   current_angle_behavior = behavior;

src/EZ-Template/drive/tracking.cpp

@@ -166,7 +166,7 @@ void Drive::ez_tracking_task() {
   r_last = r_current;
 
   // Angle and velocity
-  float t_current = -ez::util::to_rad(drive_get_angle());  // negative for math standard
+  float t_current = -ez::util::to_rad(drive_angle_get());  // negative for math standard
   float t_ = t_current - t_last;
   t_last = t_current;
 
@@ -223,7 +223,7 @@ void Drive::ez_tracking_task() {
     }
   }
 
-  odom_current.theta = drive_get_angle();
+  odom_current.theta = drive_angle_get();
 
   // This is used for PID as a "current" sensor value
   // what this value actually is doesn't matter, it just needs to move with the correct sign