From cd3d761be9ecad148ea69e0b61c9b24cc06a29f1 Mon Sep 17 00:00:00 2001
From: Michael Lachut <michael@lachut.com>
Date: Mon, 26 Feb 2024 17:06:59 -0500
Subject: [PATCH] fixed an issue with servos in the remote debugger

---
 .../ftc/teamcode/internals/debug/Debugger.kt  |  2 +-
 .../internals/hardware/accessors/Servo.kt     | 27 +++++++++++++++++++
 2 files changed, 28 insertions(+), 1 deletion(-)

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/internals/debug/Debugger.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/internals/debug/Debugger.kt
index b68bc15243ee..de6ee1bd39d4 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/internals/debug/Debugger.kt
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/internals/debug/Debugger.kt
@@ -18,7 +18,7 @@ class Debugger: Feature(), Buildable {
 
         if (HardwareGetter.opMode?.isStopRequested == true) {
             try {
-                webServer.stop(5000)
+                webServer.stop(500)
             } catch (e: InterruptedException) {
                 return
             }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/internals/hardware/accessors/Servo.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/internals/hardware/accessors/Servo.kt
index d687004692a3..1467b6b6a6c7 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/internals/hardware/accessors/Servo.kt
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/internals/hardware/accessors/Servo.kt
@@ -2,15 +2,25 @@ package org.firstinspires.ftc.teamcode.internals.hardware.accessors
 
 import com.michaell.looping.ScriptParameters
 import com.qualcomm.robotcore.hardware.Servo
+import org.firstinspires.ftc.teamcode.internals.debug.remote_debugger.RDWebSocketServer
 import org.firstinspires.ftc.teamcode.internals.hardware.HardwareGetter
+import org.firstinspires.ftc.teamcode.internals.hardware.data.MotorOperation
 import org.firstinspires.ftc.teamcode.internals.hardware.data.ServoInput
 import org.firstinspires.ftc.teamcode.internals.hardware.data.ServoOptions
+import org.firstinspires.ftc.teamcode.internals.hardware.data.StandardMotorParameters
 
 /**
  * A servo is similar to a motor but can only drive to a specific position.
  */
 class Servo(override var name: String): DeviceAccessor(name) {
 
+    init {
+        // Check if it's a special motor. If it is, add to the RD Server
+        if (standardServoID() != null) {
+            RDWebSocketServer.enableServoStatic(standardServoID()!!)
+        }
+    }
+
     /**
      * The jlooping request managing the underlying hardware.
      */
@@ -35,6 +45,23 @@ class Servo(override var name: String): DeviceAccessor(name) {
         } set(value) {
             field = value
             HardwareGetter.issueServoRequest(name, ServoInput(value, ServoOptions.SET))
+
+            if (standardServoID() != null) RDWebSocketServer.setServoPositionStatic(standardServoID()!!, value/100.0)
         }
 
+    fun standardServoID(): Int? {
+        return when (name) {
+            "servo0" -> 0
+            "servo1" -> 1
+            "servo2" -> 2
+            "servo3" -> 3
+            "servo4" -> 4
+            "servo5" -> 5
+            "servo6" -> 6
+            "servo7" -> 7
+            "servo8" -> 8
+            "servo9" -> 9
+            else -> null
+        }
+    }
 }