Add a swerve drivetrain writer

Signed-off-by: Nathan Leong <100028864@mvla.net>
Change-Id: I4750a4acba814e774befb0de4b0ba547ab67efcf
diff --git a/frc971/wpilib/falcon.h b/frc971/wpilib/falcon.h
index f6858fd..8f0f1f0 100644
--- a/frc971/wpilib/falcon.h
+++ b/frc971/wpilib/falcon.h
@@ -8,6 +8,7 @@
 #include "ctre/phoenix6/TalonFX.hpp"
 #include "glog/logging.h"
 
+#include "aos/commonmath.h"
 #include "aos/init.h"
 #include "aos/logging/logging.h"
 #include "frc971/control_loops/drivetrain/drivetrain_can_position_generated.h"
@@ -18,6 +19,7 @@
 namespace wpilib {
 
 static constexpr units::frequency::hertz_t kCANUpdateFreqHz = 200_Hz;
+static constexpr double kMaxBringupPower = 12.0;
 
 // Gets info from and writes to falcon motors using the TalonFX controller.
 class Falcon {
@@ -29,6 +31,7 @@
   void PrintConfigs();
 
   void WriteConfigs(ctre::phoenix6::signals::InvertedValue invert);
+  ctre::phoenix::StatusCode WriteCurrent(double current, double max_voltage);
 
   ctre::phoenix6::hardware::TalonFX *talon() { return &talon_; }