214 lines
7.0 KiB
C++
214 lines
7.0 KiB
C++
/*
|
|
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 "setup/initialize.hpp"
|
|
#include "EZ-Template/drive/drive.hpp"
|
|
#include "EZ-Template/sdcard.hpp"
|
|
#include "EZ-Template/util.hpp"
|
|
#include "liblvgl/llemu.hpp"
|
|
#include "pros/llemu.hpp"
|
|
#include "pros/misc.h"
|
|
|
|
// Is the PID Tuner enabled?
|
|
bool Drive::pid_tuner_enabled() { return pid_tuner_on; }
|
|
|
|
// Enable/disabling the full PID tuner
|
|
void Drive::pid_tuner_full_enable(bool enable) { is_full_pid_tuner_enabled = enable; }
|
|
bool Drive::pid_tuner_full_enabled() { return is_full_pid_tuner_enabled; }
|
|
|
|
// Toggle printing to terminal
|
|
void Drive::pid_tuner_print_terminal_set(bool input) { pid_tuner_terminal_b = input; }
|
|
bool Drive::pid_tuner_print_terminal_enabled() { return pid_tuner_terminal_b; }
|
|
|
|
// Initialize the brain screen
|
|
void Drive::pid_tuner_brain_init() {
|
|
last_auton_selector_state = ez::as::enabled();
|
|
// Shut off auton selector
|
|
ez::as::shutdown();
|
|
pros::lcd::initialize();
|
|
}
|
|
|
|
// Toggle printing to brain
|
|
void Drive::pid_tuner_print_brain_set(bool input) {
|
|
if (pid_tuner_on && input != pid_tuner_lcd_b) {
|
|
if (!pid_tuner_lcd_b) {
|
|
pid_tuner_lcd_b = input;
|
|
pid_tuner_brain_init();
|
|
pid_tuner_print_brain();
|
|
} else if (pid_tuner_lcd_b) {
|
|
pid_tuner_lcd_b = input;
|
|
if (last_auton_selector_state) {
|
|
ez::as::initialize();
|
|
} else {
|
|
pros::lcd::shutdown();
|
|
}
|
|
}
|
|
}
|
|
}
|
|
bool Drive::pid_tuner_print_brain_enabled() { return pid_tuner_lcd_b; }
|
|
|
|
// Enable PID Tuner
|
|
void Drive::pid_tuner_enable() {
|
|
pid_tuner_brain_init();
|
|
|
|
if (pid_tuner_full_enabled())
|
|
used_pid_tuner_pids = &pid_tuner_full_pids;
|
|
else
|
|
used_pid_tuner_pids = &pid_tuner_pids;
|
|
|
|
// 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;
|
|
|
|
pid_tuner_print();
|
|
}
|
|
|
|
// Disable PID Tuner
|
|
void Drive::pid_tuner_disable() {
|
|
pid_tuner_on = false;
|
|
opcontrol_curve_buttons_toggle(last_controller_curve_state);
|
|
if (last_auton_selector_state) {
|
|
ez::as::initialize();
|
|
} else {
|
|
pros::lcd::shutdown();
|
|
}
|
|
}
|
|
|
|
// Toggle PID Tuner
|
|
void Drive::pid_tuner_toggle() {
|
|
pid_tuner_on = !pid_tuner_on;
|
|
if (pid_tuner_on)
|
|
pid_tuner_enable();
|
|
else
|
|
pid_tuner_disable();
|
|
}
|
|
|
|
// Print PID Tuner
|
|
void Drive::pid_tuner_print() {
|
|
if (!pid_tuner_on) return;
|
|
|
|
double kp = used_pid_tuner_pids->at(column).consts->kp;
|
|
double ki = used_pid_tuner_pids->at(column).consts->ki;
|
|
double kd = used_pid_tuner_pids->at(column).consts->kd;
|
|
double starti = used_pid_tuner_pids->at(column).consts->start_i;
|
|
|
|
std::string sname = used_pid_tuner_pids->at(column).name + "\n";
|
|
std::string skp = "kp: " + util::to_string_with_precision(kp, util::places_after_decimal(kp, 2));
|
|
std::string ski = "ki: " + util::to_string_with_precision(ki, util::places_after_decimal(ki, 2));
|
|
std::string skd = "kd: " + util::to_string_with_precision(kd, util::places_after_decimal(kd, 2));
|
|
std::string sstarti = "start i: " + util::to_string_with_precision(starti, util::places_after_decimal(starti, 2));
|
|
|
|
skp = row == 0 ? skp + arrow : skp + "\n";
|
|
ski = row == 1 ? ski + arrow : ski + "\n";
|
|
skd = row == 2 ? skd + arrow : skd + "\n";
|
|
sstarti = row == 3 ? sstarti + arrow : sstarti + "\n";
|
|
|
|
complete_pid_tuner_output = sname + "\n" + skp + ski + skd + sstarti + "\n";
|
|
|
|
pid_tuner_print_brain();
|
|
pid_tuner_print_terminal();
|
|
}
|
|
|
|
// Print tuner to brain if it's enabled
|
|
void Drive::pid_tuner_print_brain() {
|
|
if (pid_tuner_lcd_b) ez::screen_print(complete_pid_tuner_output);
|
|
}
|
|
|
|
// Print tuner to terminal if it's enabled
|
|
void Drive::pid_tuner_print_terminal() {
|
|
if (pid_tuner_terminal_b) std::cout << complete_pid_tuner_output;
|
|
}
|
|
|
|
// Modify constants
|
|
void Drive::pid_tuner_value_modify(float p, float i, float d, float start) {
|
|
if (!pid_tuner_on) return;
|
|
|
|
switch (row) {
|
|
case 0:
|
|
used_pid_tuner_pids->at(column).consts->kp += p;
|
|
if (used_pid_tuner_pids->at(column).consts->kp < 0.0)
|
|
used_pid_tuner_pids->at(column).consts->kp = 0.0;
|
|
break;
|
|
case 1:
|
|
used_pid_tuner_pids->at(column).consts->ki += i;
|
|
if (used_pid_tuner_pids->at(column).consts->ki < 0.0)
|
|
used_pid_tuner_pids->at(column).consts->ki = 0.0;
|
|
break;
|
|
case 2:
|
|
used_pid_tuner_pids->at(column).consts->kd += d;
|
|
if (used_pid_tuner_pids->at(column).consts->kd < 0.0)
|
|
used_pid_tuner_pids->at(column).consts->kd = 0.0;
|
|
break;
|
|
case 3:
|
|
used_pid_tuner_pids->at(column).consts->start_i += start;
|
|
if (used_pid_tuner_pids->at(column).consts->start_i < 0.0)
|
|
used_pid_tuner_pids->at(column).consts->start_i = 0.0;
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
void Drive::pid_tuner_value_increase() { pid_tuner_value_modify(p_increment, i_increment, d_increment, start_i_increment); }
|
|
void Drive::pid_tuner_value_decrease() { pid_tuner_value_modify(-p_increment, -i_increment, -d_increment, -start_i_increment); }
|
|
|
|
// Set and Get increments for modifying increments
|
|
void Drive::pid_tuner_increment_p_set(double p) { p_increment = fabs(p); }
|
|
void Drive::pid_tuner_increment_i_set(double i) { i_increment = fabs(i); }
|
|
void Drive::pid_tuner_increment_d_set(double d) { d_increment = fabs(d); }
|
|
void Drive::pid_tuner_increment_start_i_set(double start_i) { start_i_increment = fabs(start_i); }
|
|
double Drive::pid_tuner_increment_p_get() { return p_increment; }
|
|
double Drive::pid_tuner_increment_i_get() { return i_increment; }
|
|
double Drive::pid_tuner_increment_d_get() { return d_increment; }
|
|
double Drive::pid_tuner_increment_start_i_get() { return start_i_increment; }
|
|
|
|
// Iterate
|
|
void Drive::pid_tuner_iterate() {
|
|
if (!pid_tuner_on) return; // Exit if it's disabled
|
|
|
|
// Exit if printing to terminal or brain are disabled
|
|
if (!pid_tuner_terminal_b && !pid_tuner_lcd_b) {
|
|
pid_tuner_disable();
|
|
printf("Cannot run PID Tuner without printing to Brain or Terminal!\n");
|
|
return;
|
|
}
|
|
|
|
// Up / Down for Rows
|
|
if (master.get_digital_new_press(pros::E_CONTROLLER_DIGITAL_RIGHT)) {
|
|
column++;
|
|
if (column > used_pid_tuner_pids->size() - 1)
|
|
column = 0;
|
|
pid_tuner_print();
|
|
} else if (master.get_digital_new_press(pros::E_CONTROLLER_DIGITAL_LEFT)) {
|
|
column--;
|
|
if (column < 0)
|
|
column = used_pid_tuner_pids->size() - 1;
|
|
pid_tuner_print();
|
|
}
|
|
|
|
// Left / Right for Columns
|
|
if (master.get_digital_new_press(pros::E_CONTROLLER_DIGITAL_DOWN)) {
|
|
row++;
|
|
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 = 3;
|
|
pid_tuner_print();
|
|
}
|
|
|
|
// Increase / Decrease constant
|
|
if (master.get_digital_new_press(pros::E_CONTROLLER_DIGITAL_A)) {
|
|
pid_tuner_value_increase();
|
|
pid_tuner_print();
|
|
} else if (master.get_digital_new_press(pros::E_CONTROLLER_DIGITAL_Y)) {
|
|
pid_tuner_value_decrease();
|
|
pid_tuner_print();
|
|
}
|
|
}
|