Add a WriteVoltage function in falcon
Signed-off-by: Maxwell Henderson <mxwhenderson@gmail.com>
Change-Id: I8640913ad99a9ecafdd13b3055a91bebee721cc9
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(),