From 4c7456e249dd2fccf2ddb32b1232e9ea6ded62c9 Mon Sep 17 00:00:00 2001
From: Ju <bangmar@iclod.com>
Date: Wed, 15 Apr 2026 20:26:08 +0000
Subject: [PATCH] fix: improve control mode handling in PhoenixNode

---
 .../include/ros_phoenix/phoenix_node.hpp      | 29 +++++++++++++------
 1 file changed, 20 insertions(+), 9 deletions(-)

diff --git a/src/HW-Devices/ros_phoenix_humble/include/ros_phoenix/phoenix_node.hpp b/src/HW-Devices/ros_phoenix_humble/include/ros_phoenix/phoenix_node.hpp
index 6e8fdb87..7464df30 100644
--- a/src/HW-Devices/ros_phoenix_humble/include/ros_phoenix/phoenix_node.hpp
+++ b/src/HW-Devices/ros_phoenix_humble/include/ros_phoenix/phoenix_node.hpp
@@ -7,10 +7,10 @@
 #include "ros_phoenix/base_node.hpp"
 
 #define Phoenix_No_WPI // remove WPI dependencies
+#include "ctre/Phoenix.h"
 #include <chrono>
 #include <stdexcept>
-
-#include "ctre/Phoenix.h"
+#include <type_traits>
 
 using namespace rclcpp;
 using namespace std::chrono_literals;
@@ -68,17 +68,28 @@ class PhoenixNode : public BaseNode {
       // CTRE library expects velocity in units/100ms
       this->controller_->Set(mode, control_msg->value / 10.0 /
                                        this->sensor_multiplier_);
-    } else if (mode == ControlMode::Position) {
+      return;
+    }
+    if (mode == ControlMode::Position) {
       this->controller_->Set(mode, (this->sensor_offset_ + control_msg->value) /
                                        this->sensor_multiplier_);
-    } else if (mode == ControlMode::PercentOutput ||
-               mode == ControlMode::Disabled) {
+      return;
+    }
+    if (mode == ControlMode::PercentOutput || mode == ControlMode::Disabled) {
       this->controller_->Set(mode, control_msg->value);
-    } else {
-      this->controller_->Set(ControlMode::Disabled, 0.0);
-      RCLCPP_WARN(this->get_logger(), "Invalid control mode: %d",
-                  static_cast<int>(mode));
+      return;
+    }
+    if constexpr (!std::is_same_v<
+                      MotorController,
+                      ctre::phoenix::motorcontrol::can::VictorSPX>) {
+      if (mode == ControlMode::Current) {
+        this->controller_->Set(mode, control_msg->value);
+        return;
+      }
     }
+    this->controller_->Set(ControlMode::Disabled, 0.0);
+    RCLCPP_WARN(this->get_logger(), "Invalid control mode: %d",
+                static_cast<int>(mode));
   }
 
   virtual rcl_interfaces::msg::SetParametersResult