Added PID Tuner #85

include/EZ-Template/drive/drive.hpp

@@ -1190,7 +1190,59 @@ class Drive {
    */
   void opcontrol_curve_buttons_iterate();
 
+  /**
+   * Enables PID Tuner
+   */
+  void pid_tuner_enable();
+
+  /**
+   * Disables PID Tuner
+   */
+  void pid_tuner_disable();
+
+  /**
+   * Toggles PID tuner between enabled and disables
+   */
+  void pid_tuner_toggle();
+
+  /**
+   * Checks if PID Tuner is enabled.  True is enabled, false is disables.
+   */
+  bool pid_tuner_enabled();
+
+  /**
+   * Iterates through controller inputs to modify PID constants
+   */
+  void pid_tuner_iterate();
+
+  /**
+   * Toggle for printing the display of the PID Tuner to the brain
+   *
+   * \param input
+   *        true prints to brain, false doesn't
+   */
+  void pid_tuner_print_brain(bool input);
+
+  /**
+   * Toggle for printing the display of the PID Tuner to the terminal
+   *
+   * \param input
+   *        true prints to terminal, false doesn't
+   */
+  void pid_tuner_print_terminal(bool input);
+
+  /**
+   * Returns true if printing to terminal is enabled
+   */
+  bool pid_tuner_print_terminal_enabled();
+
+  /**
+   * Returns true if printing to brain is enabled
+   */
+  bool pid_tuner_print_brain_enabled();
+
  private:  // !Auton
+  void print_pid_tuner();
   bool drive_toggle = true;
   bool print_toggle = true;
   int swing_min = 0;
@@ -1200,6 +1252,28 @@ class Drive {
   bool slew_swing_fwd_using_angle = false;
   bool slew_swing_rev_using_angle = false;
   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 modify_pid_tuner_value(double p, double i, double d, double start);
+  void increase_pid_tuner();
+  void decrease_pid_tuner();
+  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 pid_tuner_on = false;
+  ;
 
   /**
    * Private wait until for drive

src/EZ-Template/drive/pid_tuner.cpp

@@ -0,0 +1,141 @@
+/*
+This Source Code Form is subject to the terms of the Mozilla Public
+License, v. 2.0. If a copy of the MPL was not distributed with this
+file, You can obtain one at http://mozilla.org/MPL/2.0/.
+*/
+
+#include "EZ-Template/api.hpp"
+#include "EZ-Template/sdcard.hpp"
+#include "pros/misc.h"
+
+bool Drive::pid_tuner_enabled() { return pid_tuner_on; }
+
+void Drive::pid_tuner_print_terminal(bool input) { pid_tuner_terminal_b = input; }
+bool Drive::pid_tuner_print_terminal_enabled() { return pid_tuner_terminal_b; }
+
+void Drive::pid_tuner_print_brain(bool input) { pid_tuner_lcd_b = input; }
+bool Drive::pid_tuner_print_brain_enabled() { return pid_tuner_lcd_b; }
+
+void Drive::pid_tuner_enable() {
+  constants = {
+      {"Drive Forward PID Constants", &forward_drivePID.constants},
+      {"Drive Backward PID Constants", &backward_drivePID.constants},
+      {"Heading PID Constants", &headingPID.constants},
+      {"Turn PID Constants", &turnPID.constants},
+      {"Swing Forward PID Constants", &forward_swingPID.constants},
+      {"Swing Backward PID Constants", &backward_swingPID.constants}};
+  column_max = constants.size() - 1;
+
+  // Shut off auton selector
+  ez::as::shutdown();
+  pros::lcd::initialize();
+
+  // Keep track of the last state of this so we can set it back once PID Tuner is disables
+  last_controller_curve_state = opcontrol_curve_buttons_toggle_get();
+  opcontrol_curve_buttons_toggle(false);
+  pid_tuner_on = true;
+
+  print_pid_tuner();
+}
+
+void Drive::pid_tuner_disable() {
+  pid_tuner_on = false;
+  opcontrol_curve_buttons_toggle(last_controller_curve_state);
+  ez::as::initialize();
+}
+
+void Drive::pid_tuner_toggle() {
+  pid_tuner_on = !pid_tuner_on;
+  if (pid_tuner_on)
+    pid_tuner_enable();
+  else
+    pid_tuner_disable();
+}
+
+void Drive::print_pid_tuner() {
+  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);
+
+  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;
+
+  std::string complete = name + "\n" + kp + ki + kd + starti + "\n";
+
+  if (pid_tuner_terminal_b) std::cout << complete;
+  if (pid_tuner_lcd_b) ez::screen_print(complete);
+}
+
+void Drive::modify_pid_tuner_value(double p, double i, double d, double start) {
+  if (!pid_tuner_on) return;
+
+  switch (row) {
+    case 0:
+      constants[column].consts->kp += p;
+      break;
+    case 1:
+      constants[column].consts->ki += i;
+      break;
+    case 2:
+      constants[column].consts->kd += d;
+      break;
+    case 3:
+      constants[column].consts->start_i += start;
+      break;
+    default:
+      break;
+  }
+}
+
+void Drive::increase_pid_tuner() { modify_pid_tuner_value(0.1, 0.001, 0.25, 1); }
+void Drive::decrease_pid_tuner() { modify_pid_tuner_value(-0.1, -0.001, -0.25, -1); }
+
+void Drive::pid_tuner_iterate() {
+  if (!pid_tuner_on) return;
+
+  if (!pid_tuner_terminal_b && !pid_tuner_lcd_b) {
+    pid_tuner_disable();
+    printf("Cannot run PID Tuner without printing to Brain or Terminal!");
+  }
+
+  // Up / Down for Rows
+  if (master.get_digital_new_press(pros::E_CONTROLLER_DIGITAL_RIGHT)) {
+    column++;
+    if (column > column_max)
+      column = 0;
+    print_pid_tuner();
+  } else if (master.get_digital_new_press(pros::E_CONTROLLER_DIGITAL_LEFT)) {
+    column--;
+    if (column < 0)
+      column = column_max;
+    print_pid_tuner();
+  }
+
+  // Left / Right for Columns
+  if (master.get_digital_new_press(pros::E_CONTROLLER_DIGITAL_DOWN)) {
+    row++;
+    if (row > row_max)
+      row = 0;
+    print_pid_tuner();
+  } else if (master.get_digital_new_press(pros::E_CONTROLLER_DIGITAL_UP)) {
+    row--;
+    if (row < 0)
+      row = row_max;
+    print_pid_tuner();
+  }
+
+  // Increase / Decrease constant
+  if (master.get_digital_new_press(pros::E_CONTROLLER_DIGITAL_A)) {
+    increase_pid_tuner();
+    print_pid_tuner();
+  } else if (master.get_digital_new_press(pros::E_CONTROLLER_DIGITAL_Y)) {
+    decrease_pid_tuner();
+    print_pid_tuner();
+  }
+}

src/EZ-Template/drive/set_pid.cpp

@@ -103,6 +103,9 @@ void Drive::pid_targets_reset() {
   forward_drivePID.target_set(0);
   backward_drivePID.target_set(0);
   turnPID.target_set(0);
+  swingPID.target_set(0);
+  forward_swingPID.target_set(0);
+  backward_swingPID.target_set(0);
 }
 
 void Drive::drive_angle_set(double angle) {

src/EZ-Template/drive/user_input.cpp

@@ -145,10 +145,16 @@ void Drive::button_press(button_* input_name, int button, std::function<void()>
 }
 
 // Toggle modifying curves with controller
-void Drive::opcontrol_curve_buttons_toggle(bool toggle) { disable_controller = toggle; }
+void Drive::opcontrol_curve_buttons_toggle(bool toggle) {
+  if (pid_tuner_on && toggle) {
+    printf("Cannot modify curve while PID Tuner is active!\n");
+    return;
+  }
+  disable_controller = toggle;
+}
 bool Drive::opcontrol_curve_buttons_toggle_get() { return disable_controller; }
 
-// Modify curves with button presses and display them to contrller
+// Modify curves with button presses and display them to controller
 void Drive::opcontrol_curve_buttons_iterate() {
   if (!disable_controller) return;  // True enables, false disables.
 
@@ -235,7 +241,6 @@ void Drive::opcontrol_joystick_threshold_iterate(int l_stick, int r_stick) {
   }
 }
 
-
 // Clip joysticks based on joystick threshold
 int Drive::clipped_joystick(int joystick) { return abs(joystick) < JOYSTICK_THRESHOLD ? 0 : joystick; }
 

src/EZ-Template/util.cpp

@@ -49,7 +49,7 @@ std::string get_rest_of_the_word(std::string text, int position) {
   }
   return word;
 }
-//All iance\n\nWE WIN THESE!!!!! 
+
 void screen_print(std::string text, int line) {
   int CurrAutoLine = line;
   std::vector<string> texts = {};

src/main.cpp

@@ -1,4 +1,7 @@
 #include "main.h"
+#include "EZ-Template/sdcard.hpp"
+#include "pros/llemu.hpp"
+#include "pros/misc.hpp"
 
 
 // Chassis constructor
@@ -151,6 +154,16 @@ void opcontrol() {
   chassis.drive_brake_set(MOTOR_BRAKE_COAST);
 
   while (true) {
+    // Only allow PID Tuner to be enabled when not connected to a competition switch / tournament
+    if (!pros::competition::is_connected()) {
+      if (master.get_digital_new_press(DIGITAL_X)) 
+        chassis.pid_tuner_toggle();
+
+      if (master.get_digital_new_press(DIGITAL_B)) 
+        autonomous();
+
+      chassis.pid_tuner_iterate();
+    } 
 
     chassis.opcontrol_tank(); // Tank control
     // chassis.opcontrol_arcade_standard(ez::SPLIT); // Standard split arcade