From e06a116982df72972cbf19d825fa0f9b9e4746da Mon Sep 17 00:00:00 2001
From: ary <cjansah11@gmail.com>
Date: Fri, 29 Sep 2023 08:23:23 -0400
Subject: [PATCH] properly implement pto (untested)

rtft
---
 RELENTLESS/include/globals.hpp        |  3 +++
 RELENTLESS/include/superstructure.hpp |  3 ++-
 RELENTLESS/src/globals.cpp            |  6 ++---
 RELENTLESS/src/main.cpp               | 15 ++---------
 RELENTLESS/src/superstructure.cpp     | 36 ++++++++++++++++++++++-----
 5 files changed, 40 insertions(+), 23 deletions(-)

diff --git a/RELENTLESS/include/globals.hpp b/RELENTLESS/include/globals.hpp
index 41b5d63..e8a438e 100644
--- a/RELENTLESS/include/globals.hpp
+++ b/RELENTLESS/include/globals.hpp
@@ -38,4 +38,7 @@ namespace globals {
     extern lemlib::TrackingWheel horiz_tracking_wheel;
 
     extern Drive chassis;
+
+    extern pros::Motor& cata_left;
+    extern pros::Motor& cata_right;
 }
\ No newline at end of file
diff --git a/RELENTLESS/include/superstructure.hpp b/RELENTLESS/include/superstructure.hpp
index c91eadc..42b8328 100644
--- a/RELENTLESS/include/superstructure.hpp
+++ b/RELENTLESS/include/superstructure.hpp
@@ -23,6 +23,7 @@ namespace superstruct {
 
     //- Structure methods
     void togglePto();
-    bool getPtoState();
+    void runCata();
+    void controlCata();
     
 }
\ No newline at end of file
diff --git a/RELENTLESS/src/globals.cpp b/RELENTLESS/src/globals.cpp
index 23a5ff9..951bd3a 100644
--- a/RELENTLESS/src/globals.cpp
+++ b/RELENTLESS/src/globals.cpp
@@ -22,7 +22,7 @@ namespace globals {
     pros::Rotation rot_horiz(1);
     // pros::Rotation enc_right();
     // pros::Rotation enc_theta();
-    pros::ADIAnalogOut pto_piston(1);
+    pros::ADIAnalogOut pto_piston('A');
 
     lemlib::Drivetrain_t chassis_odom {
         &left_drive,
@@ -44,6 +44,6 @@ namespace globals {
         DRIVE_RATIO
     ); 
 
-    // Misc
-
+    pros::Motor& cata_left = chassis.left_motors[3];
+    pros::Motor& cata_right = chassis.right_motors[3];
 }
\ No newline at end of file
diff --git a/RELENTLESS/src/main.cpp b/RELENTLESS/src/main.cpp
index 3c712f6..39c0fe5 100644
--- a/RELENTLESS/src/main.cpp
+++ b/RELENTLESS/src/main.cpp
@@ -81,19 +81,8 @@ void autonomous() {}
  */
 void opcontrol() {
 	while (true) {	
-		//chassis.arcade_standard(ary::SPLIT, e_curve_type::DEFAULT);
-		if (globals::master.get_digital(DIGITAL_R1)) {
-			motor_tlb.move_voltage(-12000);
-			motor_trb.move_voltage(12000);
-		} else {
-			motor_tlb.move_voltage(0);
-			motor_trb.move_voltage(0);
-		}
-
-		if (globals::master.get_digital_new_press(DIGITAL_LEFT)) {
-			togglePto();
-		}
-		
+		chassis.tank();
+		superstruct::controlCata();
 		pros::delay(20);
 	}
 }
diff --git a/RELENTLESS/src/superstructure.cpp b/RELENTLESS/src/superstructure.cpp
index 2a22a1a..2d6f416 100644
--- a/RELENTLESS/src/superstructure.cpp
+++ b/RELENTLESS/src/superstructure.cpp
@@ -57,12 +57,36 @@ namespace superstruct {
 
   // Structure methods
 
-  void togglePto() {
-    int state = (ptoEnabled) ? 1 : 0;
-    pto_piston.set_value(state);
+  void togglePto(bool toggle) {
+    ptoEnabled = toggle;
+    chassis.pto_toggle({cata_left, cata_right}, toggle);
+    pto_piston.set_value(toggle);
+
+    if (toggle) {
+      cata_left.set_brake_mode(MOTOR_BRAKE_COAST);
+      cata_right.set_brake_mode(MOTOR_BRAKE_COAST);
+    }
   }
 
-  // bool getPtoState() {
-  //   return (pto_piston.get_value() == 1) ? true : false;
-  // }
+  void runCata(double inpt) {
+    if (!ptoEnabled) return;
+    cata_left = inpt;
+    cata_right = inpt;
+  }
+
+  int lock = 0;
+  void cataControl() {
+    if (globals::master.get_digital(DIGITAL_LEFT) && lock == 0) {
+      togglePto(!ptoEnabled);
+      lock = 1;
+    } else if(!globals::master.get_digital(DIGITAL_LEFT)) {
+      lock = 0;
+    }
+
+    if (globals::master.get_digital(DIGITAL_R1)) {
+			runCata(12000);
+		} else {
+			runCata(0);
+		}
+  }
 }
\ No newline at end of file