From c546e69ca724ad8c29203b59a12d85117775db3d Mon Sep 17 00:00:00 2001
From: Zynh Ludwig <Zynh0722@gmail.com>
Date: Thu, 28 Mar 2024 02:18:39 -0700
Subject: [PATCH] swervin?

---
 src/main/cpp/cowlib/SwerveModule.cpp   | 23 +++++++++++++++++++++++
 src/main/include/cowlib/SwerveModule.h |  6 ++++++
 2 files changed, 29 insertions(+)

diff --git a/src/main/cpp/cowlib/SwerveModule.cpp b/src/main/cpp/cowlib/SwerveModule.cpp
index a6eac1e..2fb22b3 100644
--- a/src/main/cpp/cowlib/SwerveModule.cpp
+++ b/src/main/cpp/cowlib/SwerveModule.cpp
@@ -3,7 +3,11 @@
 // the WPILib BSD license file in the root directory of this project.
 
 #include "Constants.h"
+#include "frc/geometry/Rotation2d.h"
+#include "frc/kinematics/SwerveModuleState.h"
 #include "rev/CANSparkMax.h"
+#include "units/angle.h"
+#include "units/voltage.h"
 
 #include "cowlib/SwerveModule.h"
 
@@ -25,3 +29,22 @@ SwerveModule::SwerveModule(int driveMotorId, int steerMotorId, int encoderId,
   this->driveMotor.SetSmartCurrentLimit(SwerveConstants::currentLimit);
   this->steerMotor.SetSmartCurrentLimit(SwerveConstants::currentLimit);
 }
+
+void SwerveModule::drive(double speedMetersPerSecond, double angle) {
+  units::volt_t drive{(speedMetersPerSecond / maxVelocity) * maxVoltage};
+  units::volt_t steering{-pid.Calculate(
+      this->GetEncoder().convert<units::degree>().value(), angle)};
+  driveMotor.SetVoltage(drive);
+  steerMotor.SetVoltage(steering);
+}
+
+void SwerveModule::drive(frc::SwerveModuleState state) {
+  frc::SwerveModuleState optimized =
+      frc::SwerveModuleState::Optimize(state, frc::Rotation2d(GetEncoder()));
+
+  this->drive(optimized.speed.value(), optimized.angle.Degrees().value());
+};
+
+units::turn_t SwerveModule::GetEncoder() {
+  return this->encoder.GetAbsolutePosition().GetValue();
+}
diff --git a/src/main/include/cowlib/SwerveModule.h b/src/main/include/cowlib/SwerveModule.h
index 377a022..b216601 100644
--- a/src/main/include/cowlib/SwerveModule.h
+++ b/src/main/include/cowlib/SwerveModule.h
@@ -4,8 +4,10 @@
 
 #pragma once
 
+#include "units/angle.h"
 #include <ctre/phoenix6/CANcoder.hpp>
 #include <frc/controller/PIDController.h>
+#include <frc/kinematics/SwerveModuleState.h>
 #include <rev/CANSparkMax.h>
 
 class SwerveModule {
@@ -20,4 +22,8 @@ private:
 public:
   SwerveModule(int driveMotorId, int steerMotorId, int encoderId,
                bool driveInverted, double maxVelocity, double maxVoltage);
+
+  void drive(double speedMetersPerSecond, double angle);
+  void drive(frc::SwerveModuleState state);
+  units::turn_t GetEncoder();
 };