commit 1c19df9d1adb34ca795223bb1d17efb074c1a19d
author Maxwell Henderson <mxwhenderson@gmail.com> Fri Dec 22 16:50:17 2023 -0800
committer Maxwell Henderson <mxwhenderson@gmail.com> Fri Dec 22 16:53:07 2023 -0800
tree 45f4652468fa65273fa1d6b7e00ad35bbebdbf6b
parent 0a79cb6a0a5f0a72b27b81be8b6820a1410ee17c

Add a WriteVoltage function in falcon

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

frc971/wpilib/falcon.cc

diff --git a/frc971/wpilib/falcon.cc b/frc971/wpilib/falcon.cc
index c3ff26d..deeb9f8 100644
--- a/frc971/wpilib/falcon.cc
+++ b/frc971/wpilib/falcon.cc
@@ -88,8 +88,23 @@
       static_cast<units::current::ampere_t>(current));
   // Using 0_Hz here makes it a one-shot update.
   control.UpdateFreqHz = 0_Hz;
-  control.MaxAbsDutyCycle =
-      ::aos::Clip(max_voltage, -kMaxBringupPower, kMaxBringupPower) / 12.0;
+  control.MaxAbsDutyCycle = SafeSpeed(max_voltage);
+  ctre::phoenix::StatusCode status = talon()->SetControl(control);
+  if (!status.IsOK()) {
+    AOS_LOG(ERROR, "Failed to write control to falcon %d: %s: %s", device_id(),
+            status.GetName(), status.GetDescription());
+  }
+
+  return status;
+}
+
+ctre::phoenix::StatusCode Falcon::WriteVoltage(double voltage) {
+  ctre::phoenix6::controls::DutyCycleOut control(SafeSpeed(voltage));
+
+  // Using 0_Hz here makes it a one-shot update.
+  control.UpdateFreqHz = 0_Hz;
+  control.EnableFOC = true;
+
   ctre::phoenix::StatusCode status = talon()->SetControl(control);
   if (!status.IsOK()) {
     AOS_LOG(ERROR, "Failed to write control to falcon %d: %s: %s", device_id(),

frc971/wpilib/falcon.h

diff --git a/frc971/wpilib/falcon.h b/frc971/wpilib/falcon.h
index 6ea8735..804c7a0 100644
--- a/frc971/wpilib/falcon.h
+++ b/frc971/wpilib/falcon.h
@@ -42,6 +42,8 @@
   void WriteConfigs();
   ctre::phoenix::StatusCode WriteCurrent(double current, double max_voltage);
 
+  ctre::phoenix::StatusCode WriteVoltage(double voltage);
+
   ctre::phoenix6::hardware::TalonFX *talon() { return &talon_; }
 
   // The position of the Falcon output shaft is multiplied by gear_ratio
@@ -79,6 +81,10 @@
     supply_current_limit_ = supply_current_limit;
   }
 
+  static double SafeSpeed(double voltage) {
+    return (::aos::Clip(voltage, -kMaxBringupPower, kMaxBringupPower) / 12.0);
+  }
+
  private:
   ctre::phoenix6::hardware::TalonFX talon_;
   int device_id_;