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_; }