From dcd397e00715ed1b689e56ef5c0ced43de1f0ef9 Mon Sep 17 00:00:00 2001
From: jpokornyiii <jpokornyiii@gmail.com>
Date: Wed, 26 Mar 2025 10:50:09 -0400
Subject: [PATCH] [xrp] Update XRP for new SparkFun RP2350 Board (#7880)

Enables servos 3 and 4 usage on the new XRP board.
---
 simulation/halsim_xrp/src/main/native/cpp/XRP.cpp             | 4 ++++
 .../src/main/java/edu/wpi/first/wpilibj/xrp/XRPServo.java     | 4 +++-
 xrpVendordep/src/main/native/cpp/xrp/XRPServo.cpp             | 4 ++--
 3 files changed, 9 insertions(+), 3 deletions(-)

diff --git a/simulation/halsim_xrp/src/main/native/cpp/XRP.cpp b/simulation/halsim_xrp/src/main/native/cpp/XRP.cpp
index 812ebacb9b..140f696332 100644
--- a/simulation/halsim_xrp/src/main/native/cpp/XRP.cpp
+++ b/simulation/halsim_xrp/src/main/native/cpp/XRP.cpp
@@ -142,6 +142,10 @@ void XRP::HandleServoSimValueChanged(const wpi::json& data) {
     deviceId = 4;
   } else if (data["device"] == "servo2") {
     deviceId = 5;
+  } else if (data["device"] == "servo3") {
+    deviceId = 6;
+  } else if (data["device"] == "servo4") {
+    deviceId = 7;
   }
 
   if (deviceId != -1 && servoData.find("<position") != servoData.end()) {
diff --git a/xrpVendordep/src/main/java/edu/wpi/first/wpilibj/xrp/XRPServo.java b/xrpVendordep/src/main/java/edu/wpi/first/wpilibj/xrp/XRPServo.java
index 73901e4bfa..cbd8f61725 100644
--- a/xrpVendordep/src/main/java/edu/wpi/first/wpilibj/xrp/XRPServo.java
+++ b/xrpVendordep/src/main/java/edu/wpi/first/wpilibj/xrp/XRPServo.java
@@ -21,7 +21,7 @@ public class XRPServo {
 
   private static void checkDeviceAllocation(int deviceNum) {
     if (!s_simDeviceNameMap.containsKey(deviceNum)) {
-      throw new IllegalArgumentException("Invalid XRPServo device number. Should be 4-5");
+      throw new IllegalArgumentException("Invalid XRPServo device number. Should be 4-7");
     }
 
     if (s_registeredDevices.contains(deviceNum)) {
@@ -34,6 +34,8 @@ public class XRPServo {
   static {
     s_simDeviceNameMap.put(4, "servo1");
     s_simDeviceNameMap.put(5, "servo2");
+    s_simDeviceNameMap.put(6, "servo3");
+    s_simDeviceNameMap.put(7, "servo4");
   }
 
   private final SimDouble m_simPosition;
diff --git a/xrpVendordep/src/main/native/cpp/xrp/XRPServo.cpp b/xrpVendordep/src/main/native/cpp/xrp/XRPServo.cpp
index 6df18b1cb8..5637ce4e2e 100644
--- a/xrpVendordep/src/main/native/cpp/xrp/XRPServo.cpp
+++ b/xrpVendordep/src/main/native/cpp/xrp/XRPServo.cpp
@@ -15,8 +15,8 @@
 
 using namespace frc;
 
-std::map<int, std::string> XRPServo::s_simDeviceMap = {{4, "servo1"},
-                                                       {5, "servo2"}};
+std::map<int, std::string> XRPServo::s_simDeviceMap = {
+    {4, "servo1"}, {5, "servo2"}, {6, "servo3"}, {7, "servo4"}};
 
 std::set<int> XRPServo::s_registeredDevices = {};