commit f8c96892b701ade8d01271207618763ca5f6c531
author Maxwell Henderson <maxwell.henderson@mailbox.org> Wed Jun 28 19:55:59 2023 -0700
committer Maxwell Henderson <maxwell.henderson@mailbox.org> Thu Aug 10 13:06:45 2023 -0700
tree 5f91ecbfe1f39f53b04af1bd3bdcf3ea25763e15
parent 6c6faaf47f1bf7bc954b85058bec8f48f2c1be55

Add a swerve drivetrain writer

Signed-off-by: Nathan Leong <100028864@mvla.net>
Change-Id: I4750a4acba814e774befb0de4b0ba547ab67efcf

frc971/wpilib/falcon.cc

diff --git a/frc971/wpilib/falcon.cc b/frc971/wpilib/falcon.cc
index 3f551eb..6be83aa 100644
--- a/frc971/wpilib/falcon.cc
+++ b/frc971/wpilib/falcon.cc
@@ -1,6 +1,7 @@
 #include "frc971/wpilib/falcon.h"
 
 using frc971::wpilib::Falcon;
+using frc971::wpilib::kMaxBringupPower;
 
 Falcon::Falcon(int device_id, std::string canbus,
                std::vector<ctre::phoenix6::BaseStatusSignal *> *signals,
@@ -76,6 +77,23 @@
   PrintConfigs();
 }
 
+ctre::phoenix::StatusCode Falcon::WriteCurrent(double current,
+                                               double max_voltage) {
+  ctre::phoenix6::controls::TorqueCurrentFOC control(
+      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;
+  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;
+}
+
 void Falcon::SerializePosition(flatbuffers::FlatBufferBuilder *fbb) {
   control_loops::CANFalcon::Builder builder(*fbb);
   builder.add_id(device_id_);