From e8767c6b2b1514906151efba9adf6c3930482dac Mon Sep 17 00:00:00 2001
From: danielbrownmsm <daniel.brown@msmrockets.org>
Date: Sat, 24 Dec 2022 11:14:19 -0600
Subject: [PATCH] create swerve drive hardware interface per #11

---
 src/main/cpp/hardware/HardwareInterface.cpp   | 29 +++++++++++++++++++
 src/main/include/hardware/HardwareInterface.h | 14 +++++++++
 2 files changed, 43 insertions(+)

diff --git a/src/main/cpp/hardware/HardwareInterface.cpp b/src/main/cpp/hardware/HardwareInterface.cpp
index fe8c0da..f132ed1 100644
--- a/src/main/cpp/hardware/HardwareInterface.cpp
+++ b/src/main/cpp/hardware/HardwareInterface.cpp
@@ -44,6 +44,35 @@ bool SetupDrivetrainInterface(
     return true;
 }
 
+bool SetupSwerveDriveInterface(
+    std::unique_ptr<HardwareInterface> &hardware,
+    std::shared_ptr<SwerveDriveHardwareInterface> *interface) {
+    OKC_CHECK(interface != nullptr);
+    OKC_CHECK(hardware->actuators != nullptr);
+    OKC_CHECK(hardware->sensors != nullptr);
+
+    // Get actuators interface for swerve drive.
+    std::unique_ptr<ActuatorInterface> &actuators = hardware->actuators;
+    std::unique_ptr<SensorInterface> &sensors = hardware->sensors;
+
+    // set up swerve drive interface.
+    SwerveDriveHardwareInterface swerve_drive_interface = {
+        actuators->left_front_drive_motor.get(),
+        actuators->left_back_drive_motor.get(),
+        actuators->right_front_drive_motor.get(),
+        actuators->right_back_drive_motor.get(),
+        actuators->left_front_steer_motor.get(),
+        actuators->left_back_steer_motor.get(),
+        actuators->right_front_steer_motor.get(),
+        actuators->right_back_steer_motor.get(),
+
+        sensors->ahrs.get(),
+    };
+
+    // set the output interface
+    *interface = std::make_shared<SwerveDriveHardwareInterface>(swerve_drive_interface);
+}
+
 bool SetupIntakeInterface(
     std::unique_ptr<HardwareInterface> &hardware,
     std::shared_ptr<IntakeHardwareInterface> *interface) {
diff --git a/src/main/include/hardware/HardwareInterface.h b/src/main/include/hardware/HardwareInterface.h
index 89c8c20..2c84482 100644
--- a/src/main/include/hardware/HardwareInterface.h
+++ b/src/main/include/hardware/HardwareInterface.h
@@ -9,6 +9,7 @@
 
 // Subsystem I/O
 #include "io/DrivetrainIO.h"
+#include "io/SwerveDriveIO.h"
 #include "io/IntakeIO.h"
 
 typedef struct hardware_t {
@@ -36,6 +37,19 @@ bool SetupDrivetrainInterface(
     std::unique_ptr<HardwareInterface> &hardware,
     std::shared_ptr<DrivetrainHardwareInterface> *interface);
 
+/**
+ * @brief Link the Swerve drive to the hardware interfaces.
+ *
+ * @param interface
+ * @return true
+ * @return false
+ */
+bool SetupSwerveDriveInterface(
+    std::unique_ptr<HardwareInterface> &hardware,
+    std::shared_ptr<SwerveDriveHardwareInterface> *interface);
+
+
+
 /**
  * @brief Link the Intake to the hardware interfaces.
  *