commit 1c0843cbd50642939861a64d1d3c77cd89dc2b19
author Maxwell Henderson <mxwhenderson@gmail.com> Fri Dec 22 16:20:59 2023 -0800
committer Maxwell Henderson <mxwhenderson@gmail.com> Mon Dec 25 13:14:44 2023 -0800
tree 3c953cc491ec2e8c8f8c17747f9fdd29c8db2c23
parent d2517b274c870a912460e6e92a085f0943e9f85f

Remove motorcontrol from wpilib_interface

In the 2024 version of phoenix motorcontrol deprecates so we neeed to go
through every wpilib_interface where we use it and make it use falcon
objects and DutyCycleOut instead of PercentOutput

Signed-off-by: Maxwell Henderson <mxwhenderson@gmail.com>
Change-Id: I0e1e09330b74fcc0445b469f71791a185e629a1c

y2023/wpilib_interface.cc

diff --git a/y2023/wpilib_interface.cc b/y2023/wpilib_interface.cc
index abf11da..b4d3acb 100644
--- a/y2023/wpilib_interface.cc
+++ b/y2023/wpilib_interface.cc
@@ -24,8 +24,6 @@
 #undef ERROR
 
 #include "ctre/phoenix/cci/Diagnostics_CCI.h"
-#include "ctre/phoenix/motorcontrol/can/TalonFX.h"
-#include "ctre/phoenix/motorcontrol/can/TalonSRX.h"
 #include "ctre/phoenix6/TalonFX.hpp"
 
 #include "aos/commonmath.h"
@@ -716,16 +714,27 @@
   }
 
   static void WriteCan(const double voltage,
-                       ::ctre::phoenix::motorcontrol::can::TalonFX *falcon) {
-    falcon->Set(
-        ctre::phoenix::motorcontrol::ControlMode::PercentOutput,
-        std::clamp(voltage, -kMaxBringupPower, kMaxBringupPower) / 12.0);
+                       ::ctre::phoenix6::hardware::TalonFX *talon) {
+    ctre::phoenix6::controls::DutyCycleOut motor_control(SafeSpeed(voltage));
+
+    motor_control.UpdateFreqHz = 0_Hz;
+    motor_control.EnableFOC = true;
+
+    ctre::phoenix::StatusCode status = talon->SetControl(motor_control);
+
+    if (!status.IsOK()) {
+      AOS_LOG(ERROR, "Failed to write control to falcon: %s: %s",
+              status.GetName(), status.GetDescription());
+    }
   }
 
   template <typename T>
   static void WritePwm(const double voltage, T *motor) {
-    motor->SetSpeed(std::clamp(voltage, -kMaxBringupPower, kMaxBringupPower) /
-                    12.0);
+    motor->SetSpeed(SafeSpeed(voltage));
+  }
+
+  static double SafeSpeed(double voltage) {
+    return (::aos::Clip(voltage, -kMaxBringupPower, kMaxBringupPower) / 12.0);
   }
 
   ::std::unique_ptr<::frc::TalonFX> proximal_falcon_, distal_falcon_;