From 6300dcd6cd8b1bc553ccf6cc85886633b5942895 Mon Sep 17 00:00:00 2001
From: Sachet Korada <sachet.korada@gmail.com>
Date: Sun, 15 Dec 2024 23:19:52 -0500
Subject: [PATCH] Arm motion profiling :P

---
 .../opModes/tuning/arm/ArmConstraintsTuner.kt | 62 +++++++++++++++++++
 1 file changed, 62 insertions(+)
 create mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/arm/ArmConstraintsTuner.kt

diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/arm/ArmConstraintsTuner.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/arm/ArmConstraintsTuner.kt
new file mode 100644
index 0000000..5ba51ec
--- /dev/null
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/arm/ArmConstraintsTuner.kt
@@ -0,0 +1,62 @@
+package org.firstinspires.ftc.teamcode.opModes.tuning.arm
+
+import com.acmerobotics.dashboard.FtcDashboard
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode
+import com.qualcomm.robotcore.util.ElapsedTime
+import org.firstinspires.ftc.teamcode.subsystems.arm.ArmSubsystem
+import org.firstinspires.ftc.teamcode.subsystems.slides.ElevatorSubsystem
+
+@Autonomous
+class ArmConstraintsTuner : LinearOpMode() {
+    private var maxVel = 0.0
+    private var maxAccel = 0.0
+
+    private val timer = ElapsedTime(ElapsedTime.Resolution.SECONDS)
+    override fun runOpMode() {
+        telemetry = MultipleTelemetry(FtcDashboard.getInstance().telemetry, telemetry)
+        ArmSubsystem.initialize(hardwareMap)
+
+        waitForStart()
+
+        var dt = 0.0
+        var prev = 0.0
+
+        timer.reset()
+        while (opModeIsActive()) {
+            while (timer.seconds() < TIME_TO_RUN) {
+                ArmSubsystem.clockwise()
+
+                dt = timer.seconds() - dt
+
+                val curr = ElevatorSubsystem.velocity
+                val dv = curr - prev
+
+                maxVel = ArmSubsystem.velocity.coerceAtLeast(maxVel)
+                maxAccel = maxAccel.coerceAtLeast(dv / dt)
+
+                prev = curr
+
+                telemetry.addData("Velocity", ElevatorSubsystem.velocity)
+                telemetry.addData("Position", ElevatorSubsystem.position)
+
+                telemetry.addData("Max Velocity", maxVel)
+                telemetry.addData("Max Acceleration", maxAccel)
+
+                telemetry.update()
+
+                if (isStopRequested) break
+            }
+
+            telemetry.addData("Final Max Velocity", maxVel)
+            telemetry.addData("Final Max Acceleration", maxAccel)
+            telemetry.update()
+        }
+    }
+
+    companion object {
+        @JvmField
+        var TIME_TO_RUN = 0.7
+    }
+}
\ No newline at end of file