From dc0f99ad484e36e3db5cfd73826a6f0c26f135ad Mon Sep 17 00:00:00 2001
From: Zynh Ludwig <Zynh0722@gmail.com>
Date: Thu, 28 Mar 2024 02:32:31 -0700
Subject: [PATCH] positions and rotations?

---
 src/main/cpp/cowlib/SwerveModule.cpp   | 11 +++++++++++
 src/main/include/cowlib/SwerveModule.h |  4 ++++
 2 files changed, 15 insertions(+)

diff --git a/src/main/cpp/cowlib/SwerveModule.cpp b/src/main/cpp/cowlib/SwerveModule.cpp
index 2fb22b3..c06b39d 100644
--- a/src/main/cpp/cowlib/SwerveModule.cpp
+++ b/src/main/cpp/cowlib/SwerveModule.cpp
@@ -4,6 +4,7 @@
 
 #include "Constants.h"
 #include "frc/geometry/Rotation2d.h"
+#include "frc/kinematics/SwerveModulePosition.h"
 #include "frc/kinematics/SwerveModuleState.h"
 #include "rev/CANSparkMax.h"
 #include "units/angle.h"
@@ -48,3 +49,13 @@ void SwerveModule::drive(frc::SwerveModuleState state) {
 units::turn_t SwerveModule::GetEncoder() {
   return this->encoder.GetAbsolutePosition().GetValue();
 }
+
+frc::Rotation2d SwerveModule::GetRotation() {
+  return frc::Rotation2d(this->GetEncoder());
+}
+
+frc::SwerveModulePosition SwerveModule::GetPosition() {
+  return frc::SwerveModulePosition(
+      units::meter_t(this->driveMotor.GetEncoder().GetPosition()),
+      this->GetRotation());
+}
diff --git a/src/main/include/cowlib/SwerveModule.h b/src/main/include/cowlib/SwerveModule.h
index b216601..0fcd46f 100644
--- a/src/main/include/cowlib/SwerveModule.h
+++ b/src/main/include/cowlib/SwerveModule.h
@@ -4,9 +4,11 @@
 
 #pragma once
 
+#include "frc/geometry/Rotation2d.h"
 #include "units/angle.h"
 #include <ctre/phoenix6/CANcoder.hpp>
 #include <frc/controller/PIDController.h>
+#include <frc/kinematics/SwerveModulePosition.h>
 #include <frc/kinematics/SwerveModuleState.h>
 #include <rev/CANSparkMax.h>
 
@@ -26,4 +28,6 @@ public:
   void drive(double speedMetersPerSecond, double angle);
   void drive(frc::SwerveModuleState state);
   units::turn_t GetEncoder();
+  frc::Rotation2d GetRotation();
+  frc::SwerveModulePosition GetPosition();
 };