From 0ecc580117aa7e4ecd16f962ebad57f20bddf7d3 Mon Sep 17 00:00:00 2001
From: Ishaan Ghaskadbi <ishaanghaskadbi@Ishaans-MacBook-Air.local>
Date: Wed, 20 Nov 2024 10:42:15 -0500
Subject: [PATCH] Slides Subsystem PID class

---
 .../subsystems/slides/SlidesSubsytemPID.kt    | 58 +++++++++++++++++++
 1 file changed, 58 insertions(+)
 create mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/slides/SlidesSubsytemPID.kt

diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/slides/SlidesSubsytemPID.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/slides/SlidesSubsytemPID.kt
new file mode 100644
index 0000000..0ea55b2
--- /dev/null
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/slides/SlidesSubsytemPID.kt
@@ -0,0 +1,58 @@
+package org.firstinspires.ftc.teamcode.subsystems.slides
+
+import com.acmerobotics.dashboard.config.Config
+import com.arcrobotics.ftclib.controller.PIDController
+import com.arcrobotics.ftclib.hardware.motors.Motor
+import com.arcrobotics.ftclib.hardware.motors.Motor.GoBILDA
+import com.arcrobotics.ftclib.hardware.motors.MotorGroup
+import org.firstinspires.ftc.teamcode.utils.PIDSubsystem
+import kotlin.math.PI
+
+@Config
+class SlidesSubsytemPID(
+    elevatorLeft : Motor,
+    elevatorRight : Motor
+) : PIDSubsystem(
+    PIDController(
+        0.0,
+        0.0,
+        0.0,
+    )
+) {
+    private val extendMotors = MotorGroup(elevatorLeft, elevatorRight)
+
+    val slideAngle: Double
+        get() = extendMotors.positions[0] / GoBILDA.RPM_435.cpr * 2 * PI
+
+    val slideVelocity: Double
+        get() = extendMotors.velocities[0] / GoBILDA.RPM_435.cpr * 2 * PI
+
+    init {
+        elevatorLeft.inverted = true
+        elevatorLeft.encoder.setDirection(Motor.Direction.REVERSE)
+
+        extendMotors.resetEncoder()
+        extendMotors.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE)
+    }
+
+    override fun useOutput(output: Double, setpoint: Double) {
+        // For tuning only
+        controller.setPIDF(kP, kI, kD, 0.0)
+
+        extendMotors.set(output)
+    }
+
+    override fun getMeasurement() = slideAngle
+
+    companion object {
+
+        @JvmField
+        var kP = 0.0
+
+        @JvmField
+        var kI = 0.0
+
+        @JvmField
+        var kD = 0.0
+    }
+}
\ No newline at end of file