From d404d7339f74ce1e644b4bcaad1fffe9a5ee30c6 Mon Sep 17 00:00:00 2001
From: ary <cjansah11@gmail.com>
Date: Tue, 12 Dec 2023 08:05:55 -0500
Subject: [PATCH] adjust constants, tune exit conditions

---
 CLOUDS/include/superstructure.hpp |  9 ++++++---
 CLOUDS/src/superstructure.cpp     | 22 ++++++++++++++++++----
 2 files changed, 24 insertions(+), 7 deletions(-)

diff --git a/CLOUDS/include/superstructure.hpp b/CLOUDS/include/superstructure.hpp
index 319673b..dde2c79 100644
--- a/CLOUDS/include/superstructure.hpp
+++ b/CLOUDS/include/superstructure.hpp
@@ -1,8 +1,8 @@
 #include "main.h"
 
-#define DRIVE_SPEED 120
-#define TURN_SPEED 105
-#define SWING_SPEED 90
+#define DRIVE_SPEED 127
+#define TURN_SPEED 120
+#define SWING_SPEED 120
 // R1 -> WINGS, L1 -> CATA, L2 -> PTO, R2 -> INTAKE
 // Renu's control preferences
 #define RENU_PTO_TOGGLE DIGITAL_R1
@@ -10,6 +10,8 @@
 #define RENU_INTAKE_CONTROL_INTAKE DIGITAL_L1
 #define RENU_INTAKE_CONTROL_OUTTAKE DIGITAL_L2
 #define RENU_WING_CONTROL DIGITAL_R2
+#define RENU_CLIMB_CONTROL_ONE DIGITAL_Y
+#define RENU_CLIMB_CONTROL_TWO DIGITAL_B
 
 // Ria's control preferences
 #define RIA_PTO_TOGGLE DIGITAL_LEFT
@@ -52,6 +54,7 @@ namespace superstruct {
     void runIntake(double inpt);
     void subsysControl(pros::controller_digital_e_t ptoToggleButton, pros::controller_digital_e_t cataRunButton, pros::controller_digital_e_t intakeButton, pros::controller_digital_e_t outtakeButton);
     void wingsControl(pros::controller_digital_e_t wingControlButton);
+    void actuateClimb();
 
     /* Controls */
     void renu_control();
diff --git a/CLOUDS/src/superstructure.cpp b/CLOUDS/src/superstructure.cpp
index 3f50858..868339f 100644
--- a/CLOUDS/src/superstructure.cpp
+++ b/CLOUDS/src/superstructure.cpp
@@ -44,14 +44,14 @@ namespace superstruct {
 
   // Adjust exit conditions to allow for quick movements
   void configureExitConditions() {
-    chassis.set_exit_condition(chassis.turn_exit, 50, 2, 220, 3, 500, 500);
-    chassis.set_exit_condition(chassis.swing_exit, 100, 3, 500, 7, 500, 500);
-    chassis.set_exit_condition(chassis.drive_exit, 40, 80, 300, 150, 500, 500);
+    chassis.set_exit_condition(chassis.turn_exit, 50, 2, 220, 3, 150, 500);
+    chassis.set_exit_condition(chassis.swing_exit, 100, 3, 500, 7, 150, 500);
+    chassis.set_exit_condition(chassis.drive_exit, 40, 80, 300, 150, 150, 500);
   }
 
   // Adjust PID constants for accurate movements
   void configureConstants() {
-    chassis.set_slew_min_power(80, 80);
+    chassis.set_slew_min_power(100, 100);
     chassis.set_slew_distance(7, 7);
     chassis.set_pid_constants(&chassis.headingPID, 16, 0, 32, 0);
     chassis.set_pid_constants(&chassis.forward_drivePID, 0.5, 0, 5, 0);
@@ -198,21 +198,35 @@ namespace superstruct {
     }
   }
 
+  void actuateClimb() {
+    climb_piston_one.set_value(1);
+    climb_piston_two.set_value(1);
+  }
+
+  void climbControl(pros::controller_digital_e_t but1, pros::controller_digital_e_t but2) {
+    if (globals::master.get_digital(but1) && globals::master.get_digital(but2)) {
+      actuateClimb();
+    }
+  }
+
   /*
     Controls -> For whoever is controlling the robot
   */
   void renu_control() {
     subsysControl(RENU_PTO_TOGGLE, RENU_CATA_CONTROL, RENU_INTAKE_CONTROL_INTAKE, RENU_INTAKE_CONTROL_OUTTAKE);
     wingsControl(RENU_WING_CONTROL);
+    climbControl(RENU_CLIMB_CONTROL_ONE, RENU_CLIMB_CONTROL_TWO);
   }
   
   void ria_control() {
     subsysControl(RENU_PTO_TOGGLE, RENU_CATA_CONTROL, RENU_INTAKE_CONTROL_INTAKE, RENU_INTAKE_CONTROL_OUTTAKE);
     wingsControl(RENU_WING_CONTROL);
+    climbControl(RENU_CLIMB_CONTROL_ONE, RENU_CLIMB_CONTROL_TWO);
   }
 
   void chris_control() {
     subsysControl(RENU_PTO_TOGGLE, RENU_CATA_CONTROL, RENU_INTAKE_CONTROL_INTAKE, RENU_INTAKE_CONTROL_OUTTAKE);
     wingsControl(RENU_WING_CONTROL);
+    climbControl(RENU_CLIMB_CONTROL_ONE, RENU_CLIMB_CONTROL_TWO);
   }
 }
\ No newline at end of file