From ae218b1a42c41834a80f63757a2cb603f77399e7 Mon Sep 17 00:00:00 2001 From: ary Date: Tue, 10 Oct 2023 08:10:03 -0400 Subject: [PATCH] update a lot of stuff --- .../src/arylib/drive/user_input.cpp | 2 +- RELENTLESS/include/ary-lib/PID.hpp | 2 +- RELENTLESS/include/globals.hpp | 5 +- RELENTLESS/include/superstructure.hpp | 1 - RELENTLESS/src/ary-lib/PID.cpp | 23 +++---- RELENTLESS/src/globals.cpp | 9 +-- RELENTLESS/src/main.cpp | 14 +++- RELENTLESS/src/wings.cpp | 68 ++++++++----------- 8 files changed, 59 insertions(+), 65 deletions(-) diff --git a/RELENTLESS-Lite/src/arylib/drive/user_input.cpp b/RELENTLESS-Lite/src/arylib/drive/user_input.cpp index 32b60cd..a10e838 100644 --- a/RELENTLESS-Lite/src/arylib/drive/user_input.cpp +++ b/RELENTLESS-Lite/src/arylib/drive/user_input.cpp @@ -182,7 +182,7 @@ void Drive::tank() { int r_stick = left_curve_function(master.get_analog(ANALOG_RIGHT_Y)); // Set robot to l_stick and r_stick, check joystick threshold, set active brake - joy_thresh_opcontrol(l_stick, r_stick); + joy_thresh_opcontrol(l_stick * JOYSTICK_DRIVE_SCALE, r_stick * JOYSTICK_DRIVE_SCALE); } // Arcade standarddouble JOYSTICK_DRIVE_SCALE = 1; diff --git a/RELENTLESS/include/ary-lib/PID.hpp b/RELENTLESS/include/ary-lib/PID.hpp index adca861..3f2e664 100644 --- a/RELENTLESS/include/ary-lib/PID.hpp +++ b/RELENTLESS/include/ary-lib/PID.hpp @@ -168,7 +168,7 @@ class PID { double cur; double error; double target; - double prev_error; + double lastError; double integral; double derivative; long time; diff --git a/RELENTLESS/include/globals.hpp b/RELENTLESS/include/globals.hpp index 4b924c2..89598ee 100644 --- a/RELENTLESS/include/globals.hpp +++ b/RELENTLESS/include/globals.hpp @@ -2,6 +2,7 @@ #include "okapi/api.hpp" #include "lemlib/api.hpp" #include "ary-lib/drive/drive.hpp" +#include "wings.h" #define TRACK_WIDTH 11.5 #define PLACEHOLDER_TC_OFFSET 2.5 @@ -32,8 +33,8 @@ namespace globals { extern pros::ADIDigitalOut pto_piston; - extern pros::ADIPort left_wing_piston; - extern pros::ADIPort right_wing_piston; + extern pros::ADIDigitalOut left_wing_piston; + extern pros::ADIDigitalOut right_wing_piston; extern lemlib::TrackingWheel vert_tracking_wheel; extern lemlib::TrackingWheel horiz_tracking_wheel; diff --git a/RELENTLESS/include/superstructure.hpp b/RELENTLESS/include/superstructure.hpp index 236c885..0b3a57c 100644 --- a/RELENTLESS/include/superstructure.hpp +++ b/RELENTLESS/include/superstructure.hpp @@ -18,7 +18,6 @@ namespace superstruct { void disableActiveBrake(); - // Movement Methods void driveAsync(double dist, bool useHeadingCorrection); void driveSync(double dist, bool useHeadingCorrection); diff --git a/RELENTLESS/src/ary-lib/PID.cpp b/RELENTLESS/src/ary-lib/PID.cpp index a29d289..a58e0f9 100644 --- a/RELENTLESS/src/ary-lib/PID.cpp +++ b/RELENTLESS/src/ary-lib/PID.cpp @@ -1,9 +1,3 @@ -/* -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 "main.h" #include "util.hpp" @@ -13,7 +7,7 @@ void PID::reset_variables() { output = 0; target = 0; error = 0; - prev_error = 0; + lastError = 0; integral = 0; time = 0; prev_time = 0; @@ -54,21 +48,24 @@ void PID::set_exit_condition(int p_small_exit_time, double p_small_error, int p_ void PID::set_target(double input) { target = input; } double PID::get_target() { return target; } -double PID::compute(double current) { - error = target - current; - derivative = error - prev_error; +double PID::compute(double dist) { + error = target - dist; // Calculate the error + derivative = error - lastError; // Calculate the derivative term + /* + If the integral constant is a non-zero value, accumulate the integral term with the calculated error. + If the sign of the error is not equal to the sign of the previously recorded error, abandon the integral term. + */ if (constants.ki != 0) { - if (fabs(error) < constants.start_i) integral += error; - if (util::sgn(error) != util::sgn(prev_error)) + if (util::sgn(error) != util::sgn(lastError)) integral = 0; } output = (error * constants.kp) + (integral * constants.ki) + (derivative * constants.kd); - prev_error = error; + lastError = error; return output; } diff --git a/RELENTLESS/src/globals.cpp b/RELENTLESS/src/globals.cpp index e25b142..f6959e5 100644 --- a/RELENTLESS/src/globals.cpp +++ b/RELENTLESS/src/globals.cpp @@ -10,7 +10,7 @@ namespace globals { pros::Motor motor_blf(2, MOTOR_GEARSET_06, true); pros::Motor motor_tlb(5, MOTOR_GEARSET_06, false); pros::Motor motor_trf(16, MOTOR_GEARSET_06, true); - pros::Motor motor_trb(3, MOTOR_GEARSET_06, true);// + pros::Motor motor_trb(3, MOTOR_GEARSET_06, true); pros::Motor motor_brf(1, MOTOR_GEARSET_06, false); pros::Motor motor_brb(4, MOTOR_GEARSET_06, false); @@ -23,11 +23,8 @@ namespace globals { pros::ADIDigitalOut pto_piston('A'); - // pros::ADIDigitalOut left_wing_piston(2); - // pros::ADIDigitalOut right_wing_piston(3); - - pros::ADIPort left_wing_piston('B', pros::E_ADI_DIGITAL_OUT); - pros::ADIPort right_wing_piston('C', pros::E_ADI_DIGITAL_OUT); + pros::ADIDigitalOut left_wing_piston('C'); + pros::ADIDigitalOut right_wing_piston('B'); lemlib::Drivetrain_t chassis_odom { &left_drive, diff --git a/RELENTLESS/src/main.cpp b/RELENTLESS/src/main.cpp index 01b5de9..d64bf87 100644 --- a/RELENTLESS/src/main.cpp +++ b/RELENTLESS/src/main.cpp @@ -2,6 +2,7 @@ #include "lemlib/api.hpp" #include "globals.hpp" #include "superstructure.hpp" +#include "wings.h" using namespace globals; using namespace superstruct; @@ -101,6 +102,17 @@ void opcontrol() { while (true) { chassis.tank(); superstruct::cataControl(); - pros::delay(ary::util::DELAY_TIME); + + if (globals::master.get_digital_new_press(DIGITAL_L2)) { + left_wing_piston.set_value(1); + right_wing_piston.set_value(1); + } + + if (globals::master.get_digital_new_press(DIGITAL_L1)) { + left_wing_piston.set_value(0); + right_wing_piston.set_value(0); + } + + pros::delay(ary::util::DELAY_TIME); } } diff --git a/RELENTLESS/src/wings.cpp b/RELENTLESS/src/wings.cpp index b926734..c7cc513 100644 --- a/RELENTLESS/src/wings.cpp +++ b/RELENTLESS/src/wings.cpp @@ -5,47 +5,35 @@ using namespace globals; -Wings::Wings() { - close(); - wingsopen = 0; +class Wings { + Wings::Wings() { + close(); + }; + + void Wings::open() { + left_wing_piston.set_value(1); + right_wing_piston.set_value(1); + } + + void Wings::close() { + left_wing_piston.set_value(0); + right_wing_piston.set_value(0); + } + + void Wings::toggleLeft(int value) { + left_wing_piston.set_value(value); + } + + void Wings::toggleRight(int value) { + right_wing_piston.set_value(value); + } + + void Wings::openFor(double duration) { + open(); + pros::delay(duration * 1000); + close(); + } }; -void Wings::open() { - left_wing_piston.set_value(1); - right_wing_piston.set_value(1); -} - -void Wings::close() { - left_wing_piston.set_value(0); - right_wing_piston.set_value(0); -} - -void Wings::toggleLeft(int value) { - left_wing_piston.set_value(value); -} - -void Wings::toggleRight(int value) { - right_wing_piston.set_value(value); -} - -void Wings::openFor(double duration) { - open(); - pros::delay(duration * 1000); - close(); -} - - -/* - 0 -> Both wings closed - 1 -> Only right wing opened - 2 -> Only left wing opened - 3-> Both wings open -*/ -std::uint8_t getState() { - // int stateLeft = left_wing_pisto; - // int stateRight = right_wing_piston.get_value(); - return 0; -} -