Add a swerve drivetrain writer
Signed-off-by: Nathan Leong <100028864@mvla.net>
Change-Id: I4750a4acba814e774befb0de4b0ba547ab67efcf
diff --git a/frc971/wpilib/swerve/swerve_module.h b/frc971/wpilib/swerve/swerve_module.h
new file mode 100644
index 0000000..534f0ce
--- /dev/null
+++ b/frc971/wpilib/swerve/swerve_module.h
@@ -0,0 +1,44 @@
+#ifndef FRC971_WPILIB_SWERVE_SWERVE_MODULE_H_
+#define FRC971_WPILIB_SWERVE_SWERVE_MODULE_H_
+
+#include "frc971/wpilib/falcon.h"
+
+namespace frc971 {
+namespace wpilib {
+namespace swerve {
+
+struct SwerveModule {
+ SwerveModule(int rotation_id, int translation_id, std::string canbus,
+ std::vector<ctre::phoenix6::BaseStatusSignal *> *signals,
+ double stator_current_limit, double supply_current_limit)
+ : rotation(std::make_shared<Falcon>(rotation_id, canbus, signals,
+ stator_current_limit,
+ supply_current_limit)),
+ translation(std::make_shared<Falcon>(translation_id, canbus, signals,
+ stator_current_limit,
+ supply_current_limit)) {}
+
+ void WriteModule(
+ const frc971::control_loops::drivetrain::swerve::SwerveModuleOutput
+ *module_output,
+ double max_voltage) {
+ double rotation_current = 0.0;
+ double translation_current = 0.0;
+
+ if (module_output != nullptr) {
+ rotation_current = module_output->rotation_current();
+ translation_current = module_output->translation_current();
+ }
+
+ rotation->WriteCurrent(rotation_current, max_voltage);
+ translation->WriteCurrent(translation_current, max_voltage);
+ }
+
+ std::shared_ptr<Falcon> rotation;
+ std::shared_ptr<Falcon> translation;
+};
+
+} // namespace swerve
+} // namespace wpilib
+} // namespace frc971
+#endif // FRC971_WPILIB_SWERVE_SWERVE_MODULE_H_