From 190d5c4c0731fc7d1fb52cd0f46e749db553ff58 Mon Sep 17 00:00:00 2001
From: Sachet Korada <sachet.korada@gmail.com>
Date: Thu, 14 Nov 2024 18:06:14 -0500
Subject: [PATCH] Dashboard telemetry

---
 .../ftc/teamcode/opModes/tuning/arm/ArmPIDTuner.kt          | 6 ++++++
 1 file changed, 6 insertions(+)

diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/arm/ArmPIDTuner.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/arm/ArmPIDTuner.kt
index c9ee7cf..caea3e9 100644
--- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/arm/ArmPIDTuner.kt
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/arm/ArmPIDTuner.kt
@@ -5,6 +5,7 @@ import com.acmerobotics.dashboard.config.Config
 import com.acmerobotics.dashboard.telemetry.MultipleTelemetry
 import com.arcrobotics.ftclib.command.CommandOpMode
 import com.arcrobotics.ftclib.command.InstantCommand
+import com.arcrobotics.ftclib.command.RunCommand
 import com.arcrobotics.ftclib.hardware.motors.Motor
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp
 import org.firstinspires.ftc.teamcode.constants.ControlBoard
@@ -29,6 +30,11 @@ class ArmPIDTuner : CommandOpMode() {
         InstantCommand({
             armSubsystem.setpoint = Math.toRadians(target)
         }).perpetually().schedule()
+
+        RunCommand({
+            telemetry.addData("Arm Angle: ", armSubsystem.armAngle)
+            telemetry.addData("Setpoint: ", target)
+        })
     }
 
     companion object {