From 1cbae5ced1e166a7bddb601030c654372612f068 Mon Sep 17 00:00:00 2001
From: ary <cjansah11@gmail.com>
Date: Fri, 29 Sep 2023 07:48:02 -0400
Subject: [PATCH] piston testing fr

read the title
---
 RELENTLESS/include/globals.hpp        |  4 ++++
 RELENTLESS/include/superstructure.hpp |  4 ++++
 RELENTLESS/src/globals.cpp            |  1 +
 RELENTLESS/src/main.cpp               |  6 ++++++
 RELENTLESS/src/superstructure.cpp     | 13 +++++++++++++
 5 files changed, 28 insertions(+)

diff --git a/RELENTLESS/include/globals.hpp b/RELENTLESS/include/globals.hpp
index 915d97f..41b5d63 100644
--- a/RELENTLESS/include/globals.hpp
+++ b/RELENTLESS/include/globals.hpp
@@ -9,6 +9,8 @@
 #define DRIVE_RATIO 0.75
 #define DRIVE_RPM 450
 
+bool ptoEnabled;
+
 namespace globals {
     extern pros::Controller master;
 
@@ -30,6 +32,8 @@ namespace globals {
     extern pros::Rotation rot_horiz;
     extern pros::Rotation enc_theta;
 
+    extern pros::ADIAnalogOut pto_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 6741f71..c91eadc 100644
--- a/RELENTLESS/include/superstructure.hpp
+++ b/RELENTLESS/include/superstructure.hpp
@@ -20,5 +20,9 @@ namespace superstruct {
     void turnChassis(double theta, double turnSpeedScale);
     void leftSwing(double theta, double swingSpeedScale);
     void rightSwing(double theta, double swingSpeedScale);
+
+    //- Structure methods
+    void togglePto();
+    bool getPtoState();
     
 }
\ No newline at end of file
diff --git a/RELENTLESS/src/globals.cpp b/RELENTLESS/src/globals.cpp
index 4bd0b8b..23a5ff9 100644
--- a/RELENTLESS/src/globals.cpp
+++ b/RELENTLESS/src/globals.cpp
@@ -22,6 +22,7 @@ namespace globals {
     pros::Rotation rot_horiz(1);
     // pros::Rotation enc_right();
     // pros::Rotation enc_theta();
+    pros::ADIAnalogOut pto_piston(1);
 
     lemlib::Drivetrain_t chassis_odom {
         &left_drive,
diff --git a/RELENTLESS/src/main.cpp b/RELENTLESS/src/main.cpp
index c281ee0..5dc267f 100644
--- a/RELENTLESS/src/main.cpp
+++ b/RELENTLESS/src/main.cpp
@@ -1,8 +1,10 @@
 #include "main.h"
 #include "lemlib/api.hpp"
 #include "globals.hpp"
+#include "superstructure.hpp"
 
 using namespace globals;
+using namespace superstruct;
 
 /**
  * A callback function for LLEMU's center button.
@@ -87,6 +89,10 @@ void opcontrol() {
 			motor_tlb.move_voltage(0);
 			motor_trb.move_voltage(0);
 		}
+
+		if (globals::master.get_digital_new_press(DIGITAL_LEFT)) {
+			togglePto();
+		}
 		
 		pros::delay(20);
 	}
diff --git a/RELENTLESS/src/superstructure.cpp b/RELENTLESS/src/superstructure.cpp
index 971f9c9..2a22a1a 100644
--- a/RELENTLESS/src/superstructure.cpp
+++ b/RELENTLESS/src/superstructure.cpp
@@ -2,6 +2,8 @@
 
 using namespace ary;
 using namespace globals;
+
+bool ptoEnabled = false;
 namespace superstruct {
   void configureExitConditions() {
     chassis.set_exit_condition(chassis.turn_exit, 100, 3, 500, 7, 500, 500);
@@ -52,4 +54,15 @@ namespace superstruct {
     //chassis.set_mode(SWING);
     chassis.set_swing(RIGHT_SWING, theta, SWING_SPEED * swingSpeedScale);
   }
+
+  // Structure methods
+
+  void togglePto() {
+    int state = (ptoEnabled) ? 1 : 0;
+    pto_piston.set_value(state);
+  }
+
+  // bool getPtoState() {
+  //   return (pto_piston.get_value() == 1) ? true : false;
+  // }
 }
\ No newline at end of file