update a lot of stuff

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;

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;

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;

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);

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;
 }

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,

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();
+
+		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);
 	}
 }

RELENTLESS/src/wings.cpp

@@ -5,47 +5,35 @@
 
 using namespace globals;
 
-Wings::Wings() {
+class Wings {
+    Wings::Wings() {
         close();
-    wingsopen = 0;
-};
+    };
 
-void Wings::open() {
+    void Wings::open() {
         left_wing_piston.set_value(1);
         right_wing_piston.set_value(1);
-}
+    }
 
-void Wings::close() {
+    void Wings::close() {
         left_wing_piston.set_value(0);
         right_wing_piston.set_value(0);
-}
+    }
 
-void Wings::toggleLeft(int value) {
+    void Wings::toggleLeft(int value) {
         left_wing_piston.set_value(value);   
-}
+    }
 
-void Wings::toggleRight(int value) {
+    void Wings::toggleRight(int value) {
         right_wing_piston.set_value(value);
-}
+    }
 
-void Wings::openFor(double duration) {
+    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;
-}
+    }
+};