Fixed small bugs, refactoring #89, #90, #91, #92)

include/EZ-Template/drive/drive.hpp

@@ -722,7 +722,7 @@ class Drive {
    * \param toggle_heading
    *        toggle for heading correction
    */
-  void pid_drive_set(double target, int speed, bool slew_on, bool toggle_heading = true);
+  void pid_drive_set(double target, int speed, bool slew_on = false, bool toggle_heading = true);
 
   /**
    * Sets the robot to turn using PID.
@@ -782,7 +782,7 @@ class Drive {
    * \param speed
    *        0 to 127, max speed during motion
    * \param opposite_speed
-   *        0 to 127, max speed of the opposite side of the drive during the swing.  This is used for arcs, and is defaulted to 0.
+   *        -127 to 127, max speed of the opposite side of the drive during the swing.  This is used for arcs, and is defaulted to 0.
    */
   void pid_swing_set(e_swing type, double target, int speed, int opposite_speed = 0, bool slew_on = false);
 
@@ -796,7 +796,7 @@ class Drive {
    * \param speed
    *        0 to 127, max speed during motion
    * \param opposite_speed
-   *        0 to 127, max speed of the opposite side of the drive during the swing.  This is used for arcs, and is defaulted to 0.
+   *        -127 to 127, max speed of the opposite side of the drive during the swing.  This is used for arcs, and is defaulted to 0.
    */
   void pid_swing_set(e_swing type, okapi::QAngle p_target, int speed, int opposite_speed = 0, bool slew_on = false);
 
@@ -810,7 +810,7 @@ class Drive {
    * \param speed
    *        0 to 127, max speed during motion
    * \param opposite_speed
-   *        0 to 127, max speed of the opposite side of the drive during the swing.  This is used for arcs, and is defaulted to 0.
+   *        -127 to 127, max speed of the opposite side of the drive during the swing.  This is used for arcs, and is defaulted to 0.
    */
   void pid_swing_relative_set(e_swing type, okapi::QAngle p_target, int speed, int opposite_speed = 0, bool slew_on = false);
 
@@ -824,7 +824,7 @@ class Drive {
    * \param speed
    *        0 to 127, max speed during motion
    * \param opposite_speed
-   *        0 to 127, max speed of the opposite side of the drive during the swing.  This is used for arcs, and is defaulted to 0.
+   *        -127 to 127, max speed of the opposite side of the drive during the swing.  This is used for arcs, and is defaulted to 0.
    */
   void pid_swing_relative_set(e_swing type, double target, int speed, int opposite_speed = 0, bool slew_on = false);
 
@@ -1305,31 +1305,26 @@ class Drive {
   bool slew_swing_using_angle = false;
   bool pid_tuner_terminal_b = false;
   bool pid_tuner_lcd_b = true;
-
   struct const_and_name {
     std::string name = "";
     PID::Constants *consts;
   };
   std::vector<const_and_name> constants;
   void pid_tuner_print();
-  void pid_tuner_value_modify(double p, double i, double d, double start);
+  void pid_tuner_value_modify(float p, float i, float d, float start);
   void pid_tuner_value_increase();
   void pid_tuner_value_decrease();
   void pid_tuner_print_brain();
   void pid_tuner_print_terminal();
   void pid_tuner_brain_init();
   int column = 0;
   int row = 0;
-  int column_max = 0;
-  const int row_max = 3;
-  std::string name, kp, ki, kd, starti;
   std::string arrow = " <--\n";
-  std::string newline = "\n";
-  bool last_controller_curve_state;
-  bool last_auton_selector_state;
+  bool last_controller_curve_state = false;
+  bool last_auton_selector_state = false;
   bool pid_tuner_on = false;
-  std::string complete_pid_tuner_output;
-  double p_increment = 0.1, i_increment = 0.001, d_increment = 0.25, start_i_increment = 1.0;
+  std::string complete_pid_tuner_output = "";
+  float p_increment = 0.1, i_increment = 0.001, d_increment = 0.25, start_i_increment = 1.0;
 
   /**
    * Private wait until for drive

src/EZ-Template/PID.cpp

@@ -99,7 +99,7 @@ void PID::exit_condition_print(ez::exit_output exit_type) {
 
 exit_output PID::exit_condition(bool print) {
   // If this function is called while all exit constants are 0, print an error
-  if (!(exit.small_error && exit.small_exit_time && exit.big_error && exit.big_exit_time && exit.velocity_exit_time && exit.mA_timeout)) {
+  if (exit.small_error == 0 && exit.small_exit_time == 0 && exit.big_error == 0 && exit.big_exit_time == 0 && exit.velocity_exit_time == 0 && exit.mA_timeout == 0) {
     exit_condition_print(ERROR_NO_CONSTANTS);
     return ERROR_NO_CONSTANTS;
   }

src/EZ-Template/drive/exit_conditions.cpp

@@ -118,10 +118,10 @@ void Drive::wait_until_drive(double target) {
   }
 
   // Calculate error between current and target (target needs to be an in between position)
-  int l_tar = l_start + target;
-  int r_tar = r_start + target;
-  int l_error = l_tar - drive_sensor_left();
-  int r_error = r_tar - drive_sensor_right();
+  double l_tar = l_start + target;
+  double r_tar = r_start + target;
+  double l_error = l_tar - drive_sensor_left();
+  double r_error = r_tar - drive_sensor_right();
   int l_sgn = util::sgn(l_error);
   int r_sgn = util::sgn(r_error);
 
@@ -166,7 +166,7 @@ void Drive::wait_until_turn_swing(double target) {
   }
 
   // Calculate error between current and target (target needs to be an in between position)
-  int g_error = target - drive_imu_get();
+  double g_error = target - drive_imu_get();
   int g_sgn = util::sgn(g_error);
 
   exit_output turn_exit = RUNNING;

src/EZ-Template/drive/pid_tasks.cpp

@@ -12,22 +12,24 @@ using namespace ez;
 void Drive::ez_auto_task() {
   while (true) {
     // Autonomous PID
-    if (drive_mode_get() == DRIVE)
-      drive_pid_task();
-    else if (drive_mode_get() == TURN)
-      turn_pid_task();
-    else if (drive_mode_get() == SWING)
-      swing_pid_task();
+    switch (drive_mode_get()) {
+      case DRIVE:
+        drive_pid_task();
+        break;
+      case TURN:
+        turn_pid_task();
+        break;
+      case SWING:
+        swing_pid_task();
+        break;
+      case DISABLE:
+        break;
+      default:
+        break;
+    }
 
     util::AUTON_RAN = drive_mode_get() != DISABLE ? true : false;
 
-    /*
-    if (pros::competition::is_autonomous() && !util::AUTON_RAN)
-      util::AUTON_RAN = true;
-    else if (!pros::competition::is_autonomous())
-      drive_mode_set(DISABLE);
-    */
-
     pros::delay(util::DELAY_TIME);
   }
 }

src/EZ-Template/drive/pid_tuner.cpp

@@ -52,7 +52,6 @@ void Drive::pid_tuner_enable() {
       {"Turn PID Constants", &turnPID.constants},
       {"Swing Forward PID Constants", &forward_swingPID.constants},
       {"Swing Backward PID Constants", &backward_swingPID.constants}};
-  column_max = constants.size() - 1;
 
   pid_tuner_brain_init();
 
@@ -88,16 +87,16 @@ void Drive::pid_tuner_toggle() {
 void Drive::pid_tuner_print() {
   if (!pid_tuner_on) return;
 
-  name = constants[column].name + "\n";
-  kp = "kp: " + std::to_string(constants[column].consts->kp);
-  ki = "ki: " + std::to_string(constants[column].consts->ki);
-  kd = "kd: " + std::to_string(constants[column].consts->kd);
-  starti = "start i: " + std::to_string(constants[column].consts->start_i);
+  std::string name = constants[column].name + "\n";
+  std::string kp = "kp: " + std::to_string(constants[column].consts->kp);
+  std::string ki = "ki: " + std::to_string(constants[column].consts->ki);
+  std::string kd = "kd: " + std::to_string(constants[column].consts->kd);
+  std::string starti = "start i: " + std::to_string(constants[column].consts->start_i);
 
-  kp = row == 0 ? kp + arrow : kp + newline;
-  ki = row == 1 ? ki + arrow : ki + newline;
-  kd = row == 2 ? kd + arrow : kd + newline;
-  starti = row == 3 ? starti + arrow : starti + newline;
+  kp = row == 0 ? kp + arrow : kp + "\n";
+  ki = row == 1 ? ki + arrow : ki + "\n";
+  kd = row == 2 ? kd + arrow : kd + "\n";
+  starti = row == 3 ? starti + arrow : starti + "\n";
 
   complete_pid_tuner_output = name + "\n" + kp + ki + kd + starti + "\n";
 
@@ -116,7 +115,7 @@ void Drive::pid_tuner_print_terminal() {
 }
 
 // Modify constants
-void Drive::pid_tuner_value_modify(double p, double i, double d, double start) {
+void Drive::pid_tuner_value_modify(float p, float i, float d, float start) {
   if (!pid_tuner_on) return;
 
   switch (row) {
@@ -163,26 +162,26 @@ void Drive::pid_tuner_iterate() {
   // Up / Down for Rows
   if (master.get_digital_new_press(pros::E_CONTROLLER_DIGITAL_RIGHT)) {
     column++;
-    if (column > column_max)
+    if (column > constants.size() - 1)
       column = 0;
     pid_tuner_print();
   } else if (master.get_digital_new_press(pros::E_CONTROLLER_DIGITAL_LEFT)) {
     column--;
     if (column < 0)
-      column = column_max;
+      column = constants.size() - 1;
     pid_tuner_print();
   }
 
   // Left / Right for Columns
   if (master.get_digital_new_press(pros::E_CONTROLLER_DIGITAL_DOWN)) {
     row++;
-    if (row > row_max)
+    if (row > 3)
       row = 0;
     pid_tuner_print();
   } else if (master.get_digital_new_press(pros::E_CONTROLLER_DIGITAL_UP)) {
     row--;
     if (row < 0)
-      row = row_max;
+      row = 3;
     pid_tuner_print();
   }
 

src/EZ-Template/drive/set_pid.cpp

@@ -194,8 +194,6 @@ void Drive::pid_turn_set(double target, int speed, bool slew_on) {
   // Initialize slew
   slew_turn.initialize(slew_on, max_speed, target, drive_imu_get());
 
-  printf("%.2f   %.2f\n\n", slew_turn.constants.distance_to_travel, slew_turn.constants.min_speed);
-
   // Run task
   drive_mode_set(TURN);
 }

src/EZ-Template/slew.cpp

@@ -26,7 +26,7 @@ slew::Constants slew::constants_get() { return constants; }  // Get constants
 
 // Initialize for the movement
 void slew::initialize(bool enabled, double maximum_speed, double target, double current) {
-  is_enabled = enabled;
+  is_enabled = maximum_speed < constants.min_speed ? false : enabled;
   max_speed = maximum_speed;
 
   sign = util::sgn(target - current);

src/main.cpp

@@ -145,6 +145,9 @@ void opcontrol() {
     // After you find values that you're happy with, you'll have to set them in auton.cpp
     if (!pros::competition::is_connected()) { 
       // Enable / Disable PID Tuner
+      //  When enabled:
+      //  * use A and Y to increment / decrement the constants
+      //  * use the arrow keys to navigate the constants
       if (master.get_digital_new_press(DIGITAL_X)) 
         chassis.pid_tuner_toggle();