Feature/adds neutral stake mech control (#130)

# PR Summary
PR Link: [Link](#130)

### Description
Adds VEX OS style current limiting and Neutral Stake arm control

### Reviewers
Tag reviewers.

- Required: @JakeWendling 

---
### Changelog
- Fixes bug where conveyor and ground pickup were included with
drivetrain current limiting
- Adds current limiting logic in ghost_control to mimic the V5 Brain.

### Testing
#### Automatic
- Lots of automatic testing in the current limiting module. All
cross-referenced with the purdue calculator which is the "official"
source
(https://wiki.purduesigbots.com/vex-electronics/vex-electronics/motors).
#### Manual
- Tested and confirmed on robot hardware, by printing currents to brain
screen and cross-referencing with the Purdue calculator.

### Documentation
[- Link any relevant
documentation](https://wiki.purduesigbots.com/vex-electronics/vex-electronics/motors)

---------

Co-authored-by: Omega Jerry <[email protected]>
Co-authored-by: JakeWendling <[email protected]>
Co-authored-by: MELISSA CRUZ <[email protected]>
Co-authored-by: Alpha Jerry <[email protected]>

01_Libraries/ghost_control/CMakeLists.txt

@@ -68,6 +68,30 @@ install(
   INCLUDES DESTINATION include
 )
 
+# Current Limiting
+add_library(v5_current_limiting SHARED src/models/v5_current_limiting.cpp)
+ament_target_dependencies(v5_current_limiting
+${DEPENDENCIES}
+)
+target_include_directories(v5_current_limiting
+PUBLIC
+$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+$<INSTALL_INTERFACE:include>)
+ament_export_targets(v5_current_limiting HAS_LIBRARY_TARGET)
+install(
+  TARGETS v5_current_limiting
+  EXPORT v5_current_limiting
+  LIBRARY DESTINATION lib
+  ARCHIVE DESTINATION lib
+  RUNTIME DESTINATION bin
+  INCLUDES DESTINATION include
+)
+
+add_executable(current_limit_sandbox test/current_limit_sandbox.cpp)
+target_link_libraries(current_limit_sandbox
+  v5_current_limiting
+)
+
 #################
 #### Install ####
 #################
@@ -85,4 +109,10 @@ target_link_libraries(test_dc_motor_model
   gtest
 )
 
+ament_add_gtest(test_v5_current_limiting test/test_v5_current_limiting.cpp)
+target_link_libraries(test_v5_current_limiting
+  v5_current_limiting  
+  gtest_main
+)
+
 ament_package()

01_Libraries/ghost_control/include/ghost_control/models/v5_current_limiting.hpp

@@ -0,0 +1,90 @@
+/*
+ *   Copyright (c) 2025 Maxx Wilson
+ *   All rights reserved.
+
+ *   Permission is hereby granted, free of charge, to any person obtaining a copy
+ *   of this software and associated documentation files (the "Software"), to deal
+ *   in the Software without restriction, including without limitation the rights
+ *   to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ *   copies of the Software, and to permit persons to whom the Software is
+ *   furnished to do so, subject to the following conditions:
+
+ *   The above copyright notice and this permission notice shall be included in all
+ *   copies or substantial portions of the Software.
+
+ *   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ *   IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ *   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ *   AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ *   LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ *   OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ *   SOFTWARE.
+ */
+
+#pragma once
+#include <math.h>
+#include <vector>
+#include <numeric>
+#include <ghost_util/math_util.hpp>
+
+namespace ghost_control
+{
+
+namespace v5_current_limiting
+{
+constexpr double MAX_CURRENT = 12800.0;
+
+double convertMotorToBatteryCurrent(double current);
+
+double convertBatteryToMotorCurrent(double current);
+
+/**
+ * @brief This replicates the current limiting logic implemented in VEX OS, as described on this page: https://wiki.purduesigbots.com/vex-electronics/vex-electronics/motors.
+ * 
+ * This is a weird black-box function, and is not user friendly at all, so we include other functions in this module to assist.
+ * This is intended just to replicate the existing logic upstream.
+ * 
+ * 
+ * Observations/Notes:
+ * If there are less than 9 motors plugged in, they all get the full 2500mA.
+ * If there are nine or more, the limits decrease.
+ * If there are 8 motors, and N additional motors with current limits set to zero, THEY STILL DECREASE, albiet by a very small amount.
+ * If there are nine or more motors plugged in, and a subset are limited to a value higher thatn the nominal decreased limit, then they are throttled to the adjusted limit.
+ * 
+ * @param active_current_limits_ma Vector containing any active current limits
+ * @param num_motors Number of motors plugged in to the brain
+ * @return std::vector<double> vector of all current limits, where first N are equal to the active current limits passed in (IF the active limits are less than the adjusted current limit)
+ */
+std::vector<double> calculateAllCurrentLimits(std::vector<double> active_current_limits_ma, int num_motors);
+
+/**
+ * @brief Given N motors plugged into the brain, and M current limits in milliAmps, this method returns what value we should limit all other motors to such that VEX OS will not throttle
+ * the desired/"active" motors.
+ * 
+ * Issue:
+ *  We have 20 motors plugged in. The following is completely ignored, because no motor can exceed the adjusted current limit without first lowering other motor values.
+ *  ```
+ *  motor_1.setCurrentLimit(2500.0);
+ *  ```
+ * 
+ * Solution:
+ *  ```
+ *  
+ *  lim = getRemainingCurrentDistributed(std::vector<double>(2500.0), 20);
+ *  motor2.setCurrentLimit(lim);
+ *  ...
+ *  motorN.setCurrentLimit(lim);
+ *  motor1.setCurrentLimit(2500.0);
+ *  ```
+ * 
+ * Now all motors are properly throttled such that motor 1 can run at full power.
+ * If some motors are disabled, they can also be added to the input vector to increase the calculated value.
+ * 
+ * @param active_current_limits_ma Vector containing any active current limits (either low or high)
+ * @param num_motors Number of motors plugged in to the brain
+ * @return double   Single value which can be set for all other motors to avoid throttling active current limits
+ */
+double getRemainingCurrentDistributed(std::vector<double> active_current_limits_ma, int num_motors);
+
+} // namespace v5_current_limiting
+} // namespace ghost_control

01_Libraries/ghost_control/src/models/v5_current_limiting.cpp

@@ -0,0 +1,134 @@
+/*
+ *   Copyright (c) 2025 Maxx Wilson
+ *   All rights reserved.
+
+ *   Permission is hereby granted, free of charge, to any person obtaining a copy
+ *   of this software and associated documentation files (the "Software"), to deal
+ *   in the Software without restriction, including without limitation the rights
+ *   to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ *   copies of the Software, and to permit persons to whom the Software is
+ *   furnished to do so, subject to the following conditions:
+
+ *   The above copyright notice and this permission notice shall be included in all
+ *   copies or substantial portions of the Software.
+
+ *   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ *   IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ *   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ *   AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ *   LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ *   OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ *   SOFTWARE.
+ */
+#include <ghost_control/models/v5_current_limiting.hpp>
+#include <iostream>
+
+namespace ghost_control
+{
+
+namespace v5_current_limiting
+{
+
+double convertMotorToBatteryCurrent(double current)
+{
+  return exp(((current / 1000) + 4.324) / 0.925);
+}
+
+double convertBatteryToMotorCurrent(double current)
+{
+  return (0.925 * log(current) - 4.324) * 1000;
+}
+
+std::vector<double> calculateAllCurrentLimits(std::vector<double> active_current_limits_ma, int num_motors)
+{
+  double default_current_limit_milliamps = convertBatteryToMotorCurrent(MAX_CURRENT / num_motors);
+  default_current_limit_milliamps = ghost_util::clamp(default_current_limit_milliamps, 0.0, 2500.0);
+
+  // Sum Current Limits
+  double limited_sum = 0.0;
+  int num_limited = 0;
+  for (const auto & lim_milliamps : active_current_limits_ma) {
+    if (lim_milliamps < default_current_limit_milliamps) {
+      limited_sum += convertMotorToBatteryCurrent(lim_milliamps);
+      num_limited++;
+    }
+  }
+
+  int num_unlimited = num_motors - num_limited;
+  double battery_current_per_motor = (MAX_CURRENT - limited_sum) / num_unlimited;
+
+  double adjusted_current_limit = convertBatteryToMotorCurrent(battery_current_per_motor);
+  adjusted_current_limit = ghost_util::clamp(adjusted_current_limit, 0.0, 2500.0);
+
+  std::vector<double> final_current_limits(num_motors, adjusted_current_limit);
+
+  for (int i = 0; i < active_current_limits_ma.size(); i++) {
+    if (active_current_limits_ma[i] < adjusted_current_limit) {
+      final_current_limits[i] = active_current_limits_ma[i];
+    }
+  }
+  return final_current_limits;
+}
+
+double getRemainingCurrentDistributed(std::vector<double> active_current_limits_ma, int num_motors)
+{
+  // Calculate the naive default limit
+  double default_current_limit_milliamps = convertBatteryToMotorCurrent(MAX_CURRENT / num_motors);
+  default_current_limit_milliamps = ghost_util::clamp(default_current_limit_milliamps, 0.0, 2500.0);
+
+  int num_distributed = num_motors - active_current_limits_ma.size();
+
+  // Gather necessary metrics on the active current limits
+  int num_over = 0;
+  double max_over_current = 0.0;
+  std::vector<double> under_currents;
+  for (const auto & lim : active_current_limits_ma) {
+    if (lim < default_current_limit_milliamps) {
+      under_currents.push_back(lim);
+    } else {
+      num_over++;
+      max_over_current = std::max(max_over_current, lim);
+    }
+  }
+
+  // Get the battery current per unregulated motor
+  // Note: motors are divided based on whether they exceed the naive threshold or not, so if the threshold is 1500, then there is
+  // no difference at the motor if we were to request 2000mA or 2500mA. This is why we take the max instead of an average.
+  auto battery_current_per_motor = convertMotorToBatteryCurrent(max_over_current);
+
+  // Invert the distributing operation and determine what all the limited motors need to add up to.
+  auto lim_current_sum = MAX_CURRENT - battery_current_per_motor * num_over;
+
+  // Remove user-specified under currents individually after transforming through exponential to battery.
+  for (const auto & current : under_currents) {
+    lim_current_sum -= convertMotorToBatteryCurrent(current);
+  }
+
+  // Finally, we have the sum of all the unspecified limited motors.
+  // We can divide to get the individual currents at the battery, and then convert to the requested motor limits.
+  lim_current_sum /= num_distributed;
+  auto distributed_current_limit = convertBatteryToMotorCurrent(lim_current_sum);
+
+  // Clamp to max current
+  distributed_current_limit = std::min(2500.0, distributed_current_limit);
+
+  // This is a patch for the corner case where we call this without any over current motors (which is non-sensical).
+  // Regardless, in that case, we want to return the correct answer, which ignores the default current limiting below.
+  if(num_over == 0){
+    return distributed_current_limit;
+  }
+
+  // For the individual limits to be valid, they must be below the nominal limit, otherwise they won't trigger the algorithm.
+  // We drop 1mA to satisfy the strict inequality.
+  distributed_current_limit = std::min(default_current_limit_milliamps-1, distributed_current_limit);
+
+  // If we ended up with 1mA below 2500.0, we aren't actually doing any current limiting, so just return full power. 
+  if(std::fabs(distributed_current_limit - 2499.0) < std::numeric_limits<float>::epsilon()){
+    return 2500.0;
+  }
+
+  return distributed_current_limit;
+}
+
+} // namespace v5_current_limiting
+} // namespace ghost_control

01_Libraries/ghost_control/test/current_limit_sandbox.cpp

@@ -0,0 +1,50 @@
+/*
+ *   Copyright (c) 2025 Maxx Wilson
+ *   All rights reserved.
+
+ *   Permission is hereby granted, free of charge, to any person obtaining a copy
+ *   of this software and associated documentation files (the "Software"), to deal
+ *   in the Software without restriction, including without limitation the rights
+ *   to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ *   copies of the Software, and to permit persons to whom the Software is
+ *   furnished to do so, subject to the following conditions:
+
+ *   The above copyright notice and this permission notice shall be included in all
+ *   copies or substantial portions of the Software.
+
+ *   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ *   IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ *   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ *   AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ *   LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ *   OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ *   SOFTWARE.
+ */
+
+#include <ghost_control/models/v5_current_limiting.hpp>
+
+using namespace ghost_control::v5_current_limiting;
+#include <iostream>
+
+int main(int argc, char * argv[])
+{
+  int num_motors = 9;
+  std::vector<double> active_current_limits{0.0};
+  auto throttled = getRemainingCurrentDistributed(active_current_limits, num_motors);
+  std::cout << "Remaining Unthrottled: " << throttled << std::endl;
+
+  std::vector<double> custom_current_limits;
+  custom_current_limits.insert(custom_current_limits.end(), active_current_limits.begin(), active_current_limits.end());
+  for (int i = 0; i < num_motors - active_current_limits.size(); i++) {
+    custom_current_limits.push_back(throttled);
+  }
+  auto result = calculateAllCurrentLimits(custom_current_limits, num_motors);
+
+
+  double sum = 0.0;
+  for (const auto & lim : result) {
+    sum += lim;
+    std::cout << lim << std::endl;
+  }
+  std::cout << "Sum: " << sum << std::endl;
+}